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ABSTRACT 


The thesis proposes a real-time control algorithm for the cooperation of a joint team 
consisting of a Quadrotor and a Ground robot for coordinated ISR missions. The intended 
application focuses on indoor environments, where Global Positioning System signals are 
unreliable or simply unavailable so that the control algorithms must rely on local sensor 
information. The thesis describes the appropriate set up of the lab and includes 
simulations using a full dynamic model of the quadrotor and robot, demonstrating the 
suitability of the implemented and the proposed control scheme into a waypoint 
navigation scenario. 

The implemented controller uses the Linear Quadratic Regulator method imposed 
into five different channels; pitch, roll, yaw, x-y position and height, configured to the 
appropriate gains for smoother following of the trajectory. The proposed control scheme 
incorporates three key aspects of autonomy; trajectory planning, trajectory following and 
collaboration of the two vehicles. Using the differentially-flat dynamics property of the 
system, the trajectory optimization is posed as a non-linear constrained optimization 
within the output space in the virtual domain, not explicitly related to the time domain. A 
suitable parameterization using a virtual argument as opposed to time is applied, which 
ensures initial and terminal constraint satisfaction. The speed profile is optimized 
independently, followed by the mapping to the time domain achieved using a speed 
factor. 
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I. INTRODUCTION 


A. GENERAL 

An Unmanned Vehicle (UV) is a power driven vehicle that can be autonomously 
or remotely operated and controlled. Normally, it can be deployed or recovered 
repeatedly with various types of payload. Umnanned Vehicles are often divided in three 
major categories, (1) Air (Unmanned Air Vehicles-UAVs), (2) Ground (Unmanned 
Ground Vehicles-UGVs) and (3) Maritime (Unmanned Maritime Vehicles- UMVs). The 
Maritime category can be divided into Surface (Unmanned Surface Vehicles-USVs) and 
Underwater (Unmanned Underwater Vehicles-UUV) functional areas. There are more 
divisions depending on whether the UVs are military or commercial oriented. If military, 
the branch and missions dictate the functional role, of which, Intelligence, Surveillance 
and Reconnaissance (ISR) missions are the most common. Many UVs are also used as 
force-multipliers. First wave strikes, aerial refueling, Maritime Interdiction Operations 
(MIO), autonomous payload recovery, search and rescue operations and medical 
purposes come to mind. 

Autonomous cooperation of multiple unmanned vehicles is a major concern for 
several of those functional roles. Various advantages are observed concerning different 
types of vehicles and missions. For example, two or more UAVs cooperating together in 
order to identify a moving target on the ground; UAVs and ground robots conducting 
ISR, with the UAV as a leader and the robot assisting as a follower or inversely. 

The dynamic modeling and control of these vehicles is the focus of this thesis. 
Rigorous analysis of the dynamics and kinematics has been done in order to set up the 
models for designing the controllers. The model of the ground robot is extracted from the 
forward and inverse kinematics of the vehicle. Using multiple sensors, there was an effort 
to apply several techniques for the control of the UVs. The optimization methods include 
the Linear Quadratic Regulator (LQR) and the use of differential flatness property of the 
vehicle’s dynamics, for the trajectory planning and optimization in the output space. 
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In real applications, like the dynamic environment of use ISR missions, quasi- 
optimal trajectories proved to be more effective, by achieving full autonomous control 
with trajectory planning and path following. 

B. MISSIONS 

There are many recent developments that concern unmanned systems. Most of 
them are used for dangerous and “dirty” human jobs. Facts reveal that a wide range of 
requirements and capabilities have been developed and fulfilled toward various warfare 
specifications, material requirements and interoperability. 

Since UVs can continuously meet the operator’s requirements and needs during or 
before the battle, it is expected in the near future that the unmanned systems will be an 
indispensable support element in a wide area of missions. Most of those listed for any 
category of UVs is in the Table 1. It is quite noticeable that some types of mission can be 
conducted by a certain type of unmanned vehicle and some others by all of them, within 
its capabilities. 

It is often more effective and desirable for a combination of multiple vehicles to 
be used in a mission. However, there is a challenge. How can this cooperation of two or 
more vehicles could be achieved successfully in real-time operations? Also, the question 
of which vehicles should be chosen for a specific operation, especially for the UAVs, 
since there are a lot of candidates? Observing the characteristics of the quadrotor 
helicopter, it seems right to compare it with some of the already developed unmanned air 
vehicles in order to prove how useful it can be toward the ISR missions and show the 
advantages of our choice. 
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Reconnaissance 

Reconnaissance 

ISR 

Precision Target Location and 

Designation 

Precision Target Location and 

Designation 

Communication / 

Navigation Network 

Node 

Signals Intelligence 

CBRNE Reconnaissance 

Inspection / 

Identification 

Special Operation Forces (SOF) 

Team Resupply 

Mine detection / 

Countermeasures 

Open Ocean Anti- 

Submarine Warfare 

(ASW) 

Battle Management 

Weaponization / Strike 

Payload Delivery 

Communication / Data Relay 

Battle Management 

CBRNE Reconnaissance 

CBRNE Reconnaissance 

Communication / Data Relay 

SOF Resupply 

Combat Search and Rescue 

Signals Intelligence 

Strike 

Weaponization / Strike 

Covert Sensor Insertion 

Littoral Surface Warfare 

Electronic Warfare 

Littoral Warfare 

Mine Countermeasures 

(MCM) 

Mine detection / 

Countermeasures 

Counter CCD Camera 

Information Operations 

Information Warfare 


Time Critical Strike 

Digital Mapping 


Digital Mapping 

Covert Sensor Insertion 


Oceanography 

Decoy / Pathfinder 


Decoy Pathfinder 

GPS Pseudolite 


Bottom Topography 

Littoral Undersea Warfare 




Table 1. Mission for Unmanned Vehicles 
























C. QUADROTOR TOWARD OTHER UAVs 


a. The limitations of fixed-wing unmanned aircrafts, as far as ISR missions 
are concerned (the inability to hover or fly at low speeds), have motivated the researchers 
to look at the use of Rotary-wing unmanned vehicles. Additionally, the ability to hover 
over an assigned position creates the opportunity to use various sensors, common or 
experimental ones, attached to the platform. They can be used to collect data, conducting 
surveillance for ISR missions. Moreover, the ability of the rotary wing UAVs for vertical 
takeoffs and landings can reduce the launch and recover footprint and eliminate the need 
of launching mechanisms like catapults and rails that is most common in any fixed-wing 
UAVs. 

b. However, the large mechanical complexity of the helicopter is major 
concern. A complex hub in the main rotor is needed for lift and for pitch alterations. 
Also, the vertical tail rotor is used to compensate for the reaction torque on the fuselage, 
comes from the main rotor. The quadrotor, however, consists of a pair of counter rotating 
rotors, so a tail rotor, is not only required but it offers better mobility and increases the 
payload capability. The rotor is also surrounding by a protective frame, so additional 
safety can be enhanced. 

c. It will be useful to compare the different configurations and mechanisms 
in UAVs that already were used for research purposes. Each of them present advantages 
and drawbacks as far as maneuverability, compactness, mechanics and aerodynamics 
complexity, payload capability and survivability is concerned. In Table 2, a small 
comparison between those different VTOL vehicle concepts is described. It is noticeable 
that the quadrotor turns out to be the best configuration among them all. 

d. However, there are some disadvantages. The quadrotor experiences 
dynamic instabilities and has higher sensitivity to disturbances. These facts increase 
difficulty to the control implementation. Also, the extra motors it carries, gives the ability 
to carry larger payload, but results in the increase of energy consumption. 
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CONCEPT 

s 

mm 


Power Cost 

2 

2 

2 

2 

1 

4 

3 

3 

Control Cost 

1 

1 

4 

2 

3 

3 

2 

1 

Payload 

Volume 

2 

2 

4 

3 

3 

1 

2 

1 

Maneuverability 

4 

2 

2 

3 

3 

1 

3 

3 

Mechanics 

Simplicity 

1 

3 

3 

1 

4 

4 

1 

1 

Aerodynamics 

Complexity 

1 

1 

1 

1 

4 

3 

1 

1 

Low Speed 

Flight 

4 

3 

4 

3 

4 

4 

2 

2 

High Speed 

Flight 

2 

4 

1 

2 

3 

1 

3 

3 

Miniaturization 

2 

3 

4 

2 

3 

1 

2 

4 

Survivability 

1 

3 

3 

1 

1 

3 

2 

3 

Stationary 

Flight 

4 

4 

4 

4 

4 

3 

1 

2 

Toted 

24 

28 

32 

24 

33 

28 

22 

24 


Table 2. VTOL Concept comparison (l=Bad, 4= Very Good). After [1], 
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D. RELATED WORK - RECENT DEVELOPMENTS 

Today, a lot of universities are focusing their research projects on quadrotor 
UAVs. At the same time, many companies have designed commercial quadrotors in a 
very efficient and effective way. Draganfly Innovations Inc. in Canada is one of the 
companies, they have produced many great products, like the quadrotor helicopter 
Draganflyer X4 (Figure 1) and the six-rotor helicopter Draganflyer X6 and Draganflyer 
RC helicopters. 

The Draganflyer X4 is a stable UAV platform [2] equipped with multiple sensors 
including gyroscopes, magnetometers, accelerometers, barometric pressure sensors and 
wireless video camera. It is used by many universities, such as the Massachusetts 
Institute of Technology (MIT) [3], Boeing Research and Technology [4], Vanderbilt 
University [5], and Concordia University in Canada [6]. 



Figure 1. Draganflyer. After [1]. 


Draganflyer RC helicopters were used in developing an autonomous Control 
System (VECPAV) shown in Figure 2 [4] by Vanderbilt University. They controlled the 
helicopter by receiving and processing position and motion data from a sensor and 
sending various control commands through a radio transmitter. As reported in [7] and [8], 
Draganflyer X6 helicopter is being widely used for police actions, too. Mesa County 
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Sheriff is the first Public Safety Agency to receive an FAA Certificate of Authorization 
(COA) to operate the Draganflyer X6 helicopter for law enforcement use in over a 3,300 
square mile area. May Regina’s police force, investigating a homicide, used the 
Draganflyer X6 UAV helicopter to obtain aerial pictures and video of the crime scene. 



Figure 2. Vanderbilt University’s Embedded Computing Platfonn. After [3]. 


Stanford university created a quadrotor helicopter platfonn (Stanford Testbed of 
Autonomous Rotorcraft for Multi-Agent Control II - STARMAC II), shown in Figure 3, 
and applied a PID controller for attitude/altitude control in outside environment where 
disturbances are unpredicted. The first efforts were not successful since the control of the 
vehicle flight was not accurate,. Further improvements of the controller are already 
considered [9]. 



Figure 3. STARMAC II quad-rotor UAV. After [8]. 
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In Ecole Polytechnique Federate De Lausanne (EPFL), a quadrotor helicopter 
called Omni-directional Stationary Flying Outstretched Robot (OS4) was designed for 
fully autonomous operation using many different control methods. Using the Lyapunov 
theory, linear controllers like Proportional and Integral Derivative (PID) and Linear 
Quadratic Regulator (LQR), backstepping and sliding mode methods, their research 
proved successful by using Integral backstepping where autonomous hovering with 
altitude control and autonomous take-off and landing were established [1]. 



Figure 4. OS4 Test Bench. After [9]. 


Moreover, Massachusetts Institute of Technology (MIT) developed a Real-time 
indoor Autonomous Vehicle test Environment (RAVEN) shown in Figure 5, consisting of 
ground UVs and UAV and a motion capture system for tracking in order to conduct 
autonomously multi-vehicle missions. It was a successfully developed and tested system 
where pilot’s assigned tasks can be performed in real-time by a single or multiple 
vehicles. The vehicles followed waypoints created through a trajectory generation 
algorithm. RAVEN allows an operator to control up to ten UVs simultaneously [10]. 



Mission Tasking and Guidance Computers 



Vehicle ff,2 


Figure 5. RAVEN Project. After [11]. 
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From 2007 the Networked Autonomous Vehicles Lab. (NAVL) of Concordia 
University used different quadrotor UAVs such as Draganflyer X4 and X-Qball from 
Quanser Innovations INC Company, as well as several wheeled robots as shown in 
Figure 6. They used various control techniques, focusing mostly on fault-tolerant 
cooperative control of these vehicles [6]. 



Figure 6. Concordia NAVL Project. After [5]. 


Research has been done by Virginia Tech University [11], only for cooperation of 
UGVs whereas team of mini ground heterogeneous autonomous vehicles (MUVs) were 
developed for urban search and rescue in both indoor and outdoor environments by 
utilizing onboard computers and multiple sensors. Autonomous navigation, search, 
tracking, localization and mapping (STLAM) as well as obstacle avoidance accomplished 
through various experiments and participation in competitions as MAGIC2010. Figure 6 
depicts the team of those UGVs and the sensors that are used in each vehicle. 



Figure 7. Virginia Tech University’s team of MUVs. After [11]. 
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Various Naval Postgraduate School (NPS) departments have conducted research 
for autonomous ground vehicles through the years. Small Robot Technology (SMART) 
program in NPS Physics department AXV LAB many prototype robotic platforms or 
military applications. One of all the platforms being developed, was called “Bender” and 
was equipped with a web-cam and several ultrasound sensors so as to move to designated 
destinations autonomously via waypoint navigation, controlled by a commercial BL2000 
microcontroller, which was programmed by Dynamic C language [12]. Another UGV, 
called “AGB” was a wheeled vehicle, created by MAJ Ben Miller U.S. Army, that 
utilized an acoustic and IR (infrared) detectors to detect motion, obtaining the capability 
to report images to remote monitoring stations via a web camera, in order to assist in the 
interdiction of IED placement [12]. Figure 8 shows these two vehicles. 



Figure 8. “Bender” (left) and “AGB” (right) NPS Autonomous Ground Vehicles. 

After [13]. 
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E. LITERATURE REVIEW 

Quadrotors have proved to be quite complex vehicles, but present many 
advantages for research, including the improvement of their control perfonnance for 
different flight conditions and through different control techniques and objectives, 
described in the following reviews [1], [13-16], 

One very common technique, used for more than two centuries is the Proportional 
(P), Integral (I), and Derivative (D) (PID) control method, where the proportional control is 
used to settle the output signal in direct proportion to the controller input, the integral is used 
to eliminate any steady state error, and the derivative reduces the overshoot of the system. 
PID techniques aiming to fault tolerant control of quadrotors have been presented in the 
following papers [1], [17-20], 

Another technique, the feedback linearization (FL) were used by Altug et al. [21] 
for altitude and Euler angles stabilization when Lee et al. [22] implemented an output 
feedback (OFB) controller with an observer to a nonlinear system so as to obtain an 
estimate for the vehicle’s velocities. Furthermore, Tayebi et al. [23] used a PD2 
(proportional and twice derivative) feedback structure in order to improve the transient 
performance and remove the disturbances, caused by the feedback linearization of the 
model. Benallegue et al. [24] showed a way to provide with insensitivity to the 
uncertainties by combining the FL method with the high-order sliding mode observer 
which acted also as an estimator of the disturbances. 

The stability of the system was always a problem, so Kanellakopoulos et al. 
introduced the backstepping method [25] so as to hold the nonlinear system [26] for the 
controller implementation. This technique became very popular through its variations. 
So, Madani and Benallegue [27] used the backstepping method to control three of the 
outputs and later combined it with the sliding mode control [28] to solve the chattering 
phenomenon successfully, while Mian et al. [29] stabilized the altitude simply by adding 
an integrator. 

Several quadrotor vehicles were controlled with Lyapunov theory [30], [31], 
linear quadratic regulator (LQR) control method [32] and sliding mode control [33]. 
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Castillo et al. [31] presented a controller design based on Lyapunov analysis using a 
nested saturation algorithm for stabilization of the altitude and the yaw. Real time 
experiments with autonomous take-off, hovering, and landing. The dynamic model of the 
quadrotor was obtained with a Lagrange approach while Bouabdallah et al. [1], [30] 
described the modeling of the OS4 quadrotor with both Langrange and Euler-Newton 
method and provided results for altitude and attitude control from all of the mentioned 
techniques, proving that the integral backstepping method was indeed more successful 
and efficient for his model. 

However, this literature mentions control techniques for test benches or quadrotor 
platforms in a laboratory environment or outside environment with limited disturbances 
and thus nonnal flight conditions. There is a possibility that various faults could happen 
during flight, like actuators and sensors outage (zero output), transient fault and bias 
failure and control surface defect. This will reduce the safety of the vehicle. Thus 
nonlinear control schemes should be considered and fault tolerant control (FTC) 
techniques are to be proposed. Based on FTC methods for aircrafts presented in [34], [35] 
thirty years ago, Zhang et al. [5] have presented various papers for FTC, with the most 
comprehensive one, from Zhang and Jiang [36] where they analyzed the background and 
different control schemes for fault detection and diagnosis (FDD), presenting also the 
current research activities and future challenges. 

Qi et al. [37] used Kalman filter to estimate the states and the parameters of the 
fault-tolerant control while Zhang and Jiang [38] introduced a two-stage adaptive Kalman 
filter for the observation of the potential faults and use of the extracted data for 
reconfiguration of the controller. 

Many simulation and experimental work has been done in Concordia University in 
Canada, using the same quadrotor platfonn. In his thesis[39], Zhang addressed a FTC 
design technique based on Lyapunov based nonlinear control techniques in order to 
produce acceptable performance for potential faults in the quadrotor, while Bilhim, in his 
own thesis[40] described a gain-scheduled PID controller for fault tolerant control of the 
quad-rotor. 
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Another concern toward the control of the quadrotor is the ability to conduct 
various tasks in real time. Yakime nk o et al. [41], brought Prof. Taranenko’s ideas (direct 
method of calculus of variations in flight dynamics problems with constraints on states and 
controls) to a new level and developed algorithms for real-time onboard calculation of quasi- 
optimal trajectories [42,] [43], [44], Various tests in combat vehicles and missiles using these 
algorithms were performed onboard 5th-generation aircraft [45], Kaminer et al. [46] used this 
method in 2006 to generate trajectories for landing approach of UAVs and assure flight 
deconfliction at maneuvering. 

Nowadays, more recent researchers are focused on this method, posing the 
trajectory planning as a nonlinear constrained optimization problem and separately the 
trajectory (path) following as different problem in order fully autonomy to be achieved. 
Using the differential flatness property of the equations of motions, this control method 
uses the optimization in the computation of a new feasible trajectory that meet the 
dynamic constrains and requirements for the maneuver of the vehicle characterizing by a 
given perfonnance index. In 2005 Yang et al. [47] addressed a time optimal control of a 
hovering quadrotor helicopter, where genetic algorithms were adopted. In 2006, Cowling 
et al. [48], proposed a complete real-time controller for autonomous control of a 
quadrotor UAV, while Bouktir et al. in 2008 [49] presented a simple direct method for 
trajectory planning of the quadrotor vehicle. Finally, the same year, Hoffmann et al. [50] 
in Stanford University, developed an autonomous vehicle trajectory tracking algorithm 
for the STARMAC platform. 
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F. 


THESIS OBJECTIVES - MOTIVATIONS 


Judging from the existed literature, the researchers have succeeded in achieving a 
quite acceptable perfonnance by applying different control techniques either with linear 
or nonlinear control theory. The limited experiments of real-time optimal techniques with 
inverse dynamics for quadrotor vehicles were the motivation to choose this quadrotor 
UAV for analysis and controller implementation. The importance of the cooperation of 
two or more vehicles for the successful completion of ISR missions was another urge for 
this thesis. The thesis begins with the analysis of the dynamic model of the two vehicles, 
a quadrotor and a ground robot, follows the derivation of the equations of motion in order 
to set up the state model that will be used for the controllers. Finally, the controller 
implementation takes place separately for each vehicle. For the quadrotor, it addresses 
LQR control technique for the four controller models that will be used for the path 
following of the trajectories that will be generated from a trajectory generator in real 
time. The trajectory generator will use quasi optimal solution for the generation of the 
trajectories through parameterization with fifth order polynomials. For the ground robot 
through forward and inverse kinematics an LQR controller will be used to set up the 
vehicle as a leader, where after The specific objectives are as follows: 

• Derive a mathematical model of the quadrotor UAV, according to its 
particular physical structure and dynamics; 

• Derive a mathematical model of the ground robot, according to forward 
and inverse kinematics; 

• Derive a unified control architecture that enables control of multiple 
heterogeneous UAV/UGV teams; 

• Develop a LQR controller for its vehicle without fault consideration; 

• Design a Trajectory Generator for the quadrotor; 

• Implement the two models in Matlab - Simulink integrated with Quarc 
software for the real-time operation of the vehicles , Qball-X4 quadrotor 
UAV and Qbot ground vehicle 

• Finally, perform simulations for the individual control of the vehicles and 
the cooperation control to analyze the performance of the design 
technique. 
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G. OUTLINE 

Having introduced the concept and the objectives of the thesis in the first chapter, 
the second chapter will address the setup of Quanser control lab, consisting of the 
experiment platforms UAV QBall X-4 and UGV Qbot and describes their sensors and 
elements and the whole set up needed for the completion of the experiment. 

Chapter III is devoted to modeling two types vehicles. It describes the quadrotor’s 
physical structure and dynamics, and derives its six degree-of-freedom nonlinear 
mathematical model. The same procedure is described for the ground robot through its 
forward and inverse kinematics for the derivation of the equation of motion. 

Chapter IV addresses the existing and proposed control architecture within the 
Quanser lab. 

The architecture and the simulation process for the cooperation of the two UVs 
and results of the lab experiments are shown in order to show the effectiveness of the 
thesis work. 

Finally, Chapter VI highlights the conclusion and recommendations for future 

work. 
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II. QUANSER LABORATORY SETUP 


A. INTRODUCTION 

The laboratory, where the experiments was carried out, is designed in such way, 
as to give the opportunity to any researcher to perform research in an indoor, safe 
environment that can easily controlled and / or reconfigured depending on the different 
demands. It is an open-architecture environment that consists of Quanser’s unmanned 
vehicles, the quadrotor UAV QBall-X4 [51] and the ground robot, Qbot [52], a single 
ground station and a localization system containing ten cameras, required for tracking the 
positions of vehicles in the lab space, since no GPS reception is available indoors. 

Every component of the lab can be operated from ground station computer, which 
is equipped with the required research software, including MATLAB / Simulink with 
real-time control software QuaRC installed and the OptiTrack Tracking Tools program 
for managing fully integrated with QuaRC, OptiTrack camera system. In addition, 
wireless communications are achieved through a wireless network adapter, inserted to the 
ground control station computer while a USB 2.0 port is used for connecting the 
OptiTrack indoor localization system. 

The Quanser, embedded avionics data acquisition card, HiQ on each vehicle, 
provides high-resolution inertial measurement sensors and motor outputs; while the 
controllers created from the operator in the host PC can be downloaded and executed on 
their embedded Gumstix Verdex target computer [53]. A laboratory illustration with a 
collage of various photographs of the camera set up is shown in Figure 9. 
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Figure 9. Implemented Laboratory illustration. 


B. QUANSER REAL-TIME CONTROL (QuaRC) SOFTWARE 

Quanser Real-time control (QuaRC) software [52] is a rapid control software for 
teaching and research applications, created in the paths of its predecessor WinCon, the 
first real-time software to run Simulink-generated code under Windows. QuaRC is 
compatible and integrated with Simulink and real-time workshop and gives the 
opportunity to the operator to design controllers, generate code and execute it avoiding 


low level programming and pure code writing. 
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QuaRC runs under Windows XP or Vista and is targeting either the host computer 
or a remote computer running either Windows or the QNX RTOS. For embedded 
computers on unmanned vehicles, QuaRC runs under QNX and the Quanser’s computer 
Gumstix Verdex. 

So, one or multiple controllers designed in Simulink are able to be converted into 
real time code through QuaRC and run on different target processors even on different 
chipsets running different operating systems. QuaRC also provides the operators with the 
ability to tune the control parameters while the model is running, allowing for rapid 
design and test iterations, with little to no recompilation required. 

The host and target components referred to as QuaRC Host and QuaRC Target in 
QuaRC software are designed to communicate even via Internet, in order the user to 
download a controller anywhere and control it from a remote location, tune and configure 
the control parameters while the controller runs, plot real-time data and save it inside 
MATLAB. 

Low level programming is eliminated by utilizing the QuaRC control software 
tool, since the number of Simulink blocks to be learned, are reduced. So the controller 
design becomes the priority avoiding interfacing issues and extreme code-writing. 

QuaRC blocks/tools are quite extensible for more systems and commands to be 
added and even allow a Simulink model to communicate with third party devices, while 
they provide the mathematical framework for controlling the various devices. The most 
important ones are: 

• Communication blocks for the supported communication protocols, like 
TCP/IP, UDP, Serial port and Shared memory. 

• Hardware-In-the-Loop (HIL) block set, an extensible hardware in-the-loop 
API used to interface with over 50 data acquisition cards. 

• the Vehicle Abstraction Layer (VAL) block set, consisting of a series of 
blocks that provide a group of high-level primitive commands to the 
operator, while the VAL deals with the vehicle hardware communication. 

• Through the use of the Virtual Reality Toolbox in Simulink and QuaRC, 
an interactive 3D environment with haptic feedback can be created, allows 
for simulation or training applications. 
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C. GROUND CONTROL STATION 

The ground control station is comprised of a PC which is equipped with the 
required research software, including MATLAB / Simulink with real-time control 
software QuaRC and the OptiTrack Tracking Tools program the OptiTrack camera 
system. In addition, wireless communications are achieved through a wireless network 
adapter, inserted to the ground control station computer while a USB 2.0 port is used for 
connecting the OptiTrack indoor localization system. The distributed control structure 
allows the real-time code to be generated from the host controller, wirelessly be sent 
through QuaRC to any vehicle where it would be compiled and executed on-board it. The 
ground control station can operate with several vehicle controllers at the same time. 

The researcher is able to perform several tasks through the ground control station 
computer: 

• Developing controllers for the navigation of the unmanned vehicles, 

• using the OptiTrack Tracking Tools program to calibrate the localization 
system, in order to provide wireless localization data, 

• view Simulink Scopes and several display tools to monitor the vehicles 
and their status during mission carrying out, 

• tune and update the various controller parameters to improve perfonnance 
during runtime 

• log and monitor the mission runtime data. 

• conduct vision and image processing 


D. HiQ -GUMSTIX EMBEDDED COMPUTER 

The HiQ embedded avionics data acquisition card, shown in Figure 10, developed 
by Quanser company was integrated with the small fonn-factor, lightweight Gumstix 
Verdex embedded computer [53] with wireless communications capabilities that runs a 
Linux-based operating system in order to achieve measuring the behavior of the 
unmanned systems being controlled and be able to run generated simulink models 
designed with QuaRC software. 
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The Gumstix computer has been configured as a QuaRC target system to create 
the designed controllers in the ground station computer and download and execute them 
on the QuaRC target computer without requiring embedded programming. HiQ card 
provides high-resolution inertial measurement sensors and motor outputs, and is 
responsible for the reading and writing to vehicle hardware. QuaRC’s Hardware-In-the- 
Loop (HIL) tools give directly access to the Input / Output (I/O) for any supported DAC, 
including the HiQ. 



◄-► 

10.54cm 


Figure 10. HiQ embedded avionics data acquisition card. After [55]. 

The Input / Output of the HiQ card, as it stated in [55] consists of: 

• 10 PWM outputs (servo motor outputs) 

• 3-axis gyroscope, range configurable for ±75%, ±150%, or ±300%, 
resolution 

• 0.01832%/LSB at a range setting of ±75% 

• 3-axis accelerometer, resolution 2.522 mg/LSB 

• 10 analog inputs, 12-bit, +3.3V 

• 3-axis magnetometer, 0.76923 mGa/LSB 

• 4 Maxbotix sonar inputs, 1 inch resolution 

• Serial GPS input 

• 8 channel RF receiver input 

• USB input for on-board camera (up to 9fps) 

• 2 pressure sensors, absolute and relative pressure 

• Input power 10-20V 
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E. 


LOCALIZATION SYSTEM 


Indoor laboratories have to utilize alternative localization systems in order to 
track vehicle positions in the area of the operations, since no access to GPS exists. The 
OptiTrack Tracking Tools system, used in this laboratory, is created by Natural Point 
company and designed for indoor applications. It is a camera-based localization and 
tracking system integrated with QuaRC and consists of a series of at least six motion 
tracking OptiTrack cameras connected to the ground station computer. The quantity of 
the cameras depends on the complexity of the environment; with the increase of the 
quantity to reduce the tracking loss possibility. The QuaRC OptiTrack block set allows 
operators to track multiple reflective markers simultaneously in 3-D space and/or fixed 
patterns of markers (rigid bodies) in 6-DOF position and orientation. The features of the 
OptiTrack vision system as stated in [55] are listed below: 

• Up to 16 cameras (Figure 12) can be connected and configured for single 
or multiple capture volumes, for the experiment ten cameras were chosen. 

• Capture volumes up to 400 square feet 

• Single point tracking for up to 80 markers, or 10 rigid-body objects 

• Typical calibration time is under 5 minutes 

• Position accuracy on the order of mm under typical conditions 

• USB 2.0 connectivity to ground station PC 

• Up to 100 fps tracking 



Figure 11. Natural Point OptiTrack camera, adapted from [55]. 
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More specifically, the ten cameras were chosen to be in the position shown in 
Figure 9 in order to eliminate the blind areas for every possible scenario that it will be 
taken place. The connection should have implemented through the right configuration 
since the data from the cameras can be transferred successfully if the cables are up to 5 
meters, that’s why three different hubs had to be used in specific location to fulfill that 
requirement. Each hub had to be connected to computer with uplink USB cables. Since 
those hubs are quite far away from each other, extension cables were used. Each hub can 
also connect with up to 6 cameras with high speed USB cables, although we used three or 
four only ports. The camera and the documented required configuration for the setup is 
shown in Figure 12. 



Figure 12. Optihubs Setup with Optitrack Cameras. After [55]. 
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After installing and power the whole system calibration of the cameras is needed. 
This procedure will be executed through the Optitrack Tracking Tools program, which is 
designed by the Optitrack Company and allows the operator to watch and adjust all the 
cameras’ position into the laboratory and the view coming from each of them. This 
program is compatible with Quarc software, needed for controlling the vehicles. The 
calibration procedure is actually very easy and basically two tools are needed, a trident 
and a T-shape (gamma-shape) zero point instrument. 

Firstly, the operator starts the “tracking tools” program. He has then to conduct 
two visual checks; checking that each camera is shown inside the program so that the 
connection is successful and that there is no reflection coming from the laboratory space. 
Then, he starts moving the trident in a figurate pattern (wanding process), whose edges 
have reflectors on the top, within the whole space trying to cover every possible view 
angle of each of the cameras He continues doing so until the tracking tools program 
indicates that all the cameras have calibrated successfully. Next, the zero point has to be 
adjusted with the T-shape instrument, where it shows the two axes, x and y. It is 
advisable that the vehicle has to be placed at this point in the beginning before the 
movement starts. Finally, since the calibration has completed, the calibration file has to 
be saved in order to be used in each Simulink vehicle model. 

In order to localize the vehicles, this exact file has to be used inside the Quarc’s 
software and especially the “Optitrack Point Cloud” Simulink block, a block that gives 
the position of the vehicle markers tracked by the Optitrack camera system. After that 
every model is ready to be used for any operation. Through that block you can always 
check how many markers are shown and if they are tracked successfully and in the event 
of an additional marker, thus unwanted reflection, to stop the operation and conduct 
another calibration after eliminating the issue. The cameras inside the laboratory are 
mounted accordingly and once adjusted, remain stable so that there is no need for 
continuous calibration. 
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F. QUANSER QUADROTOR Qball-X4 

1. Introduction 

The Qball-X4 quadrotor helicopter designed and constructed by the Quanser 
Company, is a rotary wing vehicle platfonn enclosed within a protective carbon fiber 
cage that is propelled by four motors fitted with 10-inch propellers. The particular design 
ensures safe operation in an indoor laboratory environment usually surrounded with 
various close range obstacles or other vehicles and it can be used in a wide variety of 
UAV research applications. 

Qball-X4 utilizes Quanser's onboard avionics data acquisition card (DAQ), called 
HiQ that cooperates with an embedded computer by Gumstix in order to read the on¬ 
board sensors of the vehicle and drive the motors. The QBall-X4 is an open-architecture 
UAV, allowing operators to rapidly create and deploy unmanned vehicle controllers 
ranging from low-level flight dynamics stabilization to advanced multi-agent guidance, 
navigation and control algorithms. Other specifications of the QBall-X4 include: 

• Diameter 0.7m - height 0.6m 

• 2 LiPo batteries, 2500mAh, 3-cell 

• 15 minute flight time per charge 

• (740Kv) motors fitted with 10 inch propellers 

• Protective cage made from carbon fiber enclosing the quadrotor 

• USB camera up to 9 frames per sec color images 

• Wireless communications capability 

QUARC, Quanser’s real-time control software, allows the operator to rapidly 
develop and test controllers on the host computer through a MATLAB Simulink 
interface, whereas these models can be generated and executed on the Gumstix embedded 
computer automatically in real time. At the same time operator can observe sensor 
measurements and tune the various parameters from the host ground station computer 
(PC or laptop). 
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The communication hierarchy for the operation of Qball-X4 is shown in Figure 
13. 


Target 


Wifi 

Send code to target (Gumstix) 
Send/Receive scope data, 
update runtime parameters 


QualMS ~y 




Runs model (controller) 


A 



Host 

Generate code 


HiQ and Gumstix 


Figure 13. Communication Hierarchy. After [51]. 


2. X4 Diagram 

The basic diagram of the Qball-X4 quadrotor with the axes and angles is shown in 
Figure 14, with the X axis aligned with the front of the vehicle. It is very common the tail 
or back of the vehicle, marked with colored tape, to point toward the operator and the 
positive X axis away from him when the vehicle operates. 



Figure 14. Qball-X4 axes and sign convention. After [51]. 
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3. 


Main Components 


The main components of Qball-X4 are described below [51]: 
a. Qball-X4 Protective Cage and Frame. 

Qball-X4 quadrotor rests inside a protective frame (Figure 2.4) which is a 
crossbeam structure to which the Qball-X4 components are mounted. The Qball-X4's 
protective cage is a carbon fiber structure designed to protect the frame, motors, 
propellers, embedded control module (HiQ and Gumstix) and speed controllers during 
minor collisions, since it is not intended to withstand large impacts or drops from heights 
greater than 2 meters. That’s why in order to move or lift it, someone has to carry it from 
the ends of the frame from both sides as it is shown in figure 15. 



Figure 15. Qball-X4 cage and frame. After [51] 

b. QUARC Data Acquisition Card (HiQ DAQ) - Gumstix Computer. 

The HiQ DAQ shown in Figure 2.6 is a high-resolution inertial 
measurement unit (IMU) and avionics input/output (I/O) data acquisition card that 
cooperating with the Gumstix embedded computer controls the vehicle by having inputs 
from the sensors on board and sending the motor commands. Each motor speed controller 
is connected in a specific order to one of the ten PWM servo output channels that are 
available on the HiQ, in order for the associated controllers to be operated. An optional 
daughterboard that contains additional I/O such as receiver or sonar inputs or a TTL 
serial input used for a GPS receiver. The standard servo output channels associated with 
every motor and the most common channel associated with sonar in the daughterboard 
are shown in Table 3. 
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Figure 7: HiQ DAQ-HiQ daughterboard with optional receiver inputs. After [51]. 



Motor 

Servo Output Channel 

Back 

0 

Front 

1 

Left 

2 

Right 

3 

Daughterboard 

Sonar 

0 


Table 3. Servo Output Channel 



Figure 16. HiQ cover. After [51]. 
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c. Qball-X4 Power - LiPo Batteries - Switches - Connectors 

The power to the Quadrotor (HiQ and motors) is provided by two 3-cell 
2500mAh LiPo batteries (#14 in Figure 17) which should be secured tightly in a vertical 
position with velcro straps on the bottom side of the frame. After connecting the batteries 
with the battery connectors, the power is turned on by using two switches (one for each 
battery) (#12 in Figure 17). Because of the fact that these batteries will be damaged and 
turned to no use if they are discharged below 10 V, they should be fully charged when 
they reach 10.6 V or less. 



Figure 17. Battery switch and connector. After [51]. 

d. Motors, Propellers (10x4.7) and Speed controllers (ESCs) 

The motors used in the quadrotor are four E-Flite Park 400 (740 Kv) 
motors (Figure 18) fitted with paired counter-rotating APC 10x4.7 propellers [4] (#16 in 
Figure 18), mounted and connected with the four electronic speed controllers (ESCs) [5], 
along the X and Y axes of the frame. 

The motors and propellers are configured so that the front and back motors 
spin clockwise and the left and right motors spin counterclockwise. The ESCs receive 
commands from the HiQ in the form of PWM outputs from 1ms (minimum throttle) to 
2ms (maximum throttle) or can be configured to set the throttle range, but always with 
initial PWM outputs to the minimum throttle value of 0.05. 
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Figure 18. Motor and propeller-ESCs and batteries. After [51]. 



Figure 19. Optional sonar and sonar mount. After [51]. 


ID# 

Description 

ID# 

Description 

1 

Qball-X4 protective cage 

10 

GPS serial input 

2 

Qball-X4 frame 

11 

Battery switch 

3 

HiQ DAQ with Gunistix 

12 

Battery connector 

4 

HiQ inertial measurement unit 

13 

Speed controllers (ESCs) 

5 

HiQ servo PWM outputs 

14 

LiPo batteries 

6 

HiQ cover 

15 

Optional sonar 

7 

HiQ daughterboard with optional 
receiver 

16 

Propeller (10x4.7) 

8 

Receiver inputs 

17 

Motor 

9 

Sonar inputs 




Table 4. Description of Qball’s main components. After [51]. 
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4. System Set up 


a. Wireless Connection 

The quadrotor communicates with the host computer and the ground robot 

by utilizing an ad-hoc peer-to-peer wireless TCP/IP connection through a USB wireless 

adapter. The network established is called GSAH and is an unsecured one, with the 

following settings / properties: 

IP address: 182.168.1.xxx (any unused number) 

Subnet mask: 255.255.255.0 

Default gateway: 182.168.1.xxx (same as IP) 



Figure 20. Wireless USB adapter settings 

b. Qball-X4 Vehicle Setup 

After checking that the motors, propellers are firmly secured to the frame 
regularly (after every 2 hours of flight), the batteries must be installed and connected to 
the battery connectors in the way described above and is shown in Figure 21. 
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Figure 21. Batteries secured with velcro straps. After [51]. 

To power on the Qball-X4, the two power switches connected to the 
battery cables (#11 in Figure 8) must be turned on, initiating the Gumstix wireless 
module one minute after. Then if connected to the GSAH ad-hoc network on the host PC, 
the quadrotor is ready to be navigated. 

c. Quanser Real-Time Control (QuaRC) Software Configuration 

After installing QuaRQ software, a new item will be added in Simulink 
menu. Some configurations must be done to be able to run any QuaRC model on the 
target vehicle. 

First of all, the target's Gumstix IP address must be specified. So, the 
operator has to setup the default target IP address for all targets, inside theQuaRC menu 
through the Preferences option (e.g., “tcpip://182.168.1.200:17001”). 

Moreover, “External” simulation mode must be selected instead of 
“Normal” to run the model on the target machine (Gumstix), otherwise only a simulation 
will be run on the host machine. 

Finally, the building of the model (QuaRC/Build) only remains in order to 
begin the code generation and compiling steps on the target vehicle. QUARC console 
will show when the compilation will be finished (usually this process takes some 
minutes). 
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d. Qball-X4 Sensors Associated with QuaRC Blocks 

QuaRC's open-architecture hardware and extensive Simulink block set 
provides users with powerful controls development tools. Several of these blocks of the 
QuaRQ software have to be used in order to read the sensors attached in the quadrotor 
and provided the data in Simulink models. The most important are the Hardware-In-the- 
Loop (HIL) block for communication with HiQ board. The HIL Initialize block for 
initialization the HiQ and setup the I/O parameters and the HIL Read Write block for 
reading and writing from the HiQ to the model. 

The quadrotor’s commands for the four motors are associated with the 
PWM outputs 0 to 3. The range of the PWM output values is between 0.05 (zero throttle) 
which corresponds to a 1ms pulse (5% of a 20ms duty cycle) and 0.10 (full throttle), 
equivalent to 2ms pulse (10% of a 20ms duty cycle). 

To control the flight of the quadrotor several measurements are required 
from the IMU. The input from the magnetometer, functioning as a digital compass is used 
to measure the heading (corresponding yaw angle) while the 3-axis gyroscope and 
accelerometer inputs are used to measure the quadrotor dynamics and orientation (roll, 
pitch and yaw angles). 

As already noted, the LiPo batteries should be charged at 10.6V, otherwise 
the batteries will be destroyed. The block “Show Message on Host” allows the operator 
to check the battery capacity by displaying a low battery warning message on the host 
PC. The operating capacity input measures the battery value as a percentage (0-1%) of 
the quadrotor’s input voltage range from the minimum value of 10V to the maximum one 
of 20V (10.6V is equivalent to 0.06 or 6%). 
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G. QUANSER QBOT GROUND VEHICLE 


1. Introduction 


The Quanser Qbot (Figure 22) is an autonomous ground robot vehicle consists of 
the iRobot Create platfonn widely used in robot applications with four infrared and three 
sonar sensors and a Logitech Quickcam Pro 9000 USB camera mounted on the vehicle, 
and the embedded Quanser Controller Module (QCM) utilizing a Gumstix computer so as 
to run QuaRC, Quanser's real-time control software and the Qbot data acquisition card 
(DAC). So, in other words, Quanser has taken the iRobot Create platform and augmented 
its sensor capabilities by adding [54]: 

• 8 PWM outputs for servo motors 

• 7 reconfigurable digital I/O, plus 1 digital output LED 

• 7 analog inputs, 12-bit, +5V inputs, resolution 6.2 mV 

• 5 infra-red (IR) sensors up to 150cm 

• 3 sonar sensors 15cm to 6.45m, 1-inch resolution 

• 3-axis magnetometer, resolution of 0.77 milli-Gauss 

• USB camera up to 9fps color images 

• Wireless communications 



Figure 22. The Quanser Qbot - front (Left) and top (Right) view. After [54]. 


The QCM interface is a MATLAB Simulink with QuaRC as in the quadrotor. The 
Qbot is accessible through three different block sets: the Roomba block set to drive the 
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vehicle, the HIL block set to read from sensors and/or write to servo outputs, and finally 
the OpenCV block set to access the camera. The controllers are developed in Simulink 
with QuaRC on the host computer, and these models are downloaded and compiled into 
executables on the target [54] seamlessly. A diagram of this configuration is shown in 
Figure 23 and the characteristics of the Qbot vehicle is shown in Table 5. 



Figure 23. Communication Hierarchy. After [54], 


Qbot Specifications 

Symbol 

Description 

Value 

Unit 

D 

Diameter 

0.34 

m 

H 

Height 

(With Camera Attachment) 

0.19 

m 

Vmax 

Maximum speed 

0.5 

m/sec 

M 

Total mass 

2.92 

Kg 


Table 5. Qbot Model Parameters and System Specifications. From [54] 
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2. Main Components Description 

The main components of the Qbot are described in the next section [54]. 
a. The iRobot Create® 

The Qbot uses an iRobot Create® frame (Figure 24). The Qbot body 
frame axes are the Quanser standard, where the x-axis is in the forward direction, the y- 
axis is to the left, and the z-axis is up. The iRobot Create® has a bumper sensor and an 
omni-directional infrared receiver. The QCM is capable to read these sensors and 
receiving their data. Two differential drive wheels drive the vehicle of diameter of 34 cm 
and height (without camera attachment) of 7 cm. 



6-32 Mounting 
Cavities 


DB-25 


Figure 24. Anatomy of Qbot, showing various components and body axes. 

After [54]. 

The Qbot is turned on by pressing the power button. Built-in demos for the 
vehicle are also available through the Play and Advance buttons (Figure25). 

Play 

Power Button Button Advance Button 



Power LED 


Play LED Advance LED 


Figure 25. Buttons on the Qbot Frame. After [54]. 
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b. QbotDAC 

The Qbot DAC is a data acquisition board, located underneath the black 
cover of the Qbot, existed for receiving analog inputs and sonar inputs or any other input 
from different optional sensors. It is also capable of writing PWM outputs for possible 
servo actuators. 


c. Gumstix Computer 

The Gumstix is a small-scale, fully functional, open source computer at 
where the MATLAB/ Simulink models are directly downloaded, compiled, and executed 
through the QuaRC software. The Gumstix motherboard is connected directly to the Qbot 
DAC. Wifi attachment board to allow wireless connection between the target Gumstix 
and the host computer and/or other vehicles is also available. 

d. The Printed Circuit Board (PCB) 

The wiring and circuitry for the Qbot in the printed circuit board (PCB) 
that is located on the black cover of the Qbot over the Gumstix computer and the DAC. 
The sensors and the webcam are also mounted on the PCB. Figure 26 shows the 
accessible pins for the user. In particular, the DIO, PWM output, and analog input pins 
have been labeled for clarity. 



Legend I I .Analog Input [ l PWM Output l l Digital Input Output 

□ SWnSW Jumper 1 1 EXTIXT Jumpn 


□ LEDS 

Figure 26. Qbot PCB showing available pins for the PWM output, 

Digital Input/Output and Analog Input pins, and jumpers for INT/EXT 

power. After [54]. 
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e. 


Digital Input/Output Pins (DIO #) 


The DIO channels (0 to 6) are set as inputs by default. With the HIL 
Initialize block, the operator can configure the DIO channels either as inputs or outputs 
(not both). There is also a fixed output indicated by a LED labeled DI07 associated with 
a final digital channel (7). 

f SW/nSW and INT/EXT Jumpers 

The operator can switch between internal power from the iRobot Create 
battery (INT) and an external battery power supply (EXT) with the power source jumper 
INT/EXT jumper. To power the Qbot, the jumper should be placed in the INT position 
and then the SW/nSW jumper will indicate if the iRobot Create must be switched on 
(SW) for the Qbot to receive power, or whether the Qbot should always draw power even 
when the iRobot Create is off (nSW). 

g. USB Camera 

The camera is mounted on top of the Qbot (Figure 27). The QuaRC block 
set that uses the Open Source Computer Vision library giving the opportunity to the 
operator to capture and display images in real time, process them, and even save them. 
The image Logitech Quickcam Pro 9000 USB resolution though is low. 



Figure 27. Logitech Quickcam Pro 9000 USB Camera. After [54]. 
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h. Battery 

The Qbot is powered by the Advanced Power System (APS) Battery 
(Figure 28-Left) provided by iRobot and placed in specific location under the Qbot 
(Figure 28-Right). If the battery is fully charged, is able to last continuously for about 2 
hours. The power level of the battery is designated by the power light (green for fully 
charged / red for discharged batteries) on the Qbot platform. The battery takes about 3 
hours to fully charge. 


Cliff Sensor 




Figure 28. The Qbot the Advanced Power System Battery (Left picture) - 
Battery location highlighted in the Bottom view of the Qbot. 

After [54]. 


i. Infrared Sensors - Sonar Sensors 

Qbot has five SHARP 2Y0A02 low cost infrared range sensors (20-150 
cm) (Figure 29-Left) and three MaxSonar-EZO, very short to long range detection and 
ranging, sonar sensors (Figure 29-Right), that are connected to the analog input channels 
of the Qbot DAC. Readings of those sensors can be taken through the HIL Read Write 
block of the QUARC software. The sonar provides detection range between 0 and 254- 
inches (6.45 meters) while the information provided covers range from 6-inches out to 
254-inches with resolution of 1 inch. 

* 

Figure 29. SHARP GP2D12 IR Sensor (Left) - LV-MaxSonar-EZO Sonar Range 

Finder (Right). After [54]. 
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3. System Setup 

a. Setting up the Qbot 

To set up the Qbot the operator has to follow two steps: 

(1) Insert the battery under the Qbot to the designated area. 

(2) Press the power button to turn on both the robot and Qbot 

DAC/Gumstix. 

b. Establishing Wireless Connection 

The Qbot communicates with the host computer and the quadoror by 
utilizing an ad-hoc peer-to-peer wireless TCP/IP connection through a USB wireless 
adapter. The network established is the same GSAH one, with the settings / properties 
described before for the quadrotor. 

c. Quanser Real-Time Control (QuaRC) Configuration 

Having installed QUARQ software, the same configurations with the 
quadrotor have to be done for the Qbot as well, so as to be able to run any QUARC 
model on the target vehicle. 

First of all, the target's Gumstix IP address must be specified. So, the 
operator has to setup the default target IP address for all targets, inside the QUARC menu 
through the Preferences option (e.g., “tcpip://182.168.1.200:17001”). 

Moreover, “External” simulation mode must be selected instead of 
“Normal”, so as to run the model on the target machine (Gumstix), otherwise only a 
simulation will be run on the host machine. 

Next, the building of the model (QUARC/Build) only remains in order to 
begin the code generation and compiling steps on the target vehicle. QUARC console 
will show when the compilation will be finished (usually this process takes some 
minutes). 
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d. Qbot Sensors Associated with QuaRC Blocks 

Several blocks of the QUARQ software have to be used in order to read 
the sensors attached in the Qbot and provided the data in Simulink models. The most 
important are described below [54]: 

(1) The Roomba Initialize block located in the Simulink 
Library Browser, under QuaRC Targets / Devices / Third-Party / iRobot / Roomba / 
Interfacing, is required for the Gumstix computer to communicate with the Qbot. The 
operator must change the local host from the default 2 to 1. 

(2) The HIL Initialize block located in the Simulink Library 
Browser, under QuaRC Targets / Data Acquisition / Generic / Configuration, is required 
to communicate with the Qbot DAC. When model runs, the board type "qbot" must be 
selected in order to target the Qbot. 

(3) The HIL Read Write block located in the Simulink Library 
Browser, under QuaRC Targets / Data Acquisition / Generic / Immediate I/O is required 
to read infrared sensor (analog) and sonar measurements from Qbot. 

(4) The blocks located in the Simuli nk Library Browser, under 
QuaRC Targets Beta / Image Processing / Open Source Computer Vision are required to 
use the on-board Logitech camera for image processing. 
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H. TOWARD UNIFIED CONTROL ARCHITECTURE 


From the standpoint of controls QBall and QBot are shown in Figures 30 and 31, 
respectively. On the left the control inputs that affect vehicles dynamics and kinematics 
are introduced. On the right the available output signals are shown. The position feedback 
is provided by the motion capture cameras. While both systems are capable to operate 
autonomously, the communication with the control station is provided via a wireless 
connection. Multiple systems can talk to each other directly as well. 



Quadrotor QBall-X4 Architecture 


X, Y Position 

Through Motion Canture Cameras 


Vright 


Vleft 



QBot 


Visual Camera Output 


IR Cameras Output 


Sonars Output 


Bumper Output 


STATES: X, dotX, Y dotY, 0, dote 


Wireless Connection to 
the control station 


Figure 30. Ground Robot QBot Architecture 
Table 6 shows sensors available on both vehicles side by side. 
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Vehicles 

QBall - X4 

QBot 

Sensors 



IR Camera 

- 

+ 

Visual Camera 

- 

+ 

Sonar 

+ 

+ 

Bumper 

- 

+ 

Accelerometer 

+ 

- 

Magnetometer 

+ 

- 





Table 6. Comparison of the two vehicles sensors 


QBall can also be equipped with the IR/EO camera. In tenns of performance, 

• QBall has a large velocity compare o that of QBot which opportunity leads 
to the fast accomplishment of mission. 

• The higher height of QBall operation results in larger view of the field of 
operation and better operation coverage. 

• QBot has a larger payload / sensor capability and therefore better obstacle 
avoidance and identification of the target capability. 

Hence these two vehicles complement each other, and that’s why their 
cooperation could improve the chances of mission success. 
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III. MODELLING OL QUADROTOR AND GROUND ROBOT 


In this chapter, the dynamic and kinematics modeling of the quadrotor and the 
ground robot will be presented to the extent of deriving the final equations describing the 
vehicles and that they will be used for the design of the controllers. The general dynamic 
model of the quadrotor has been presented in many papers and can be derived through 
two different approaches, the Euler - Newton and the Euler - Lagrange formalisms, see 
[1]. The modeling of a ground robot is modeled with the forward and the inverse 
kinematics; see [56] and [57]. 

A. MODELLING OF QUADROTOR QBALL-X4 

In order to start modeling the quadrotor, the coordinate frames that were used, 
have to be presented, described and defined. 

1. Coordinate Frames 

a. Earth-Fixed Inertial Frame V 

The inertial frame is established by the direction of the Earth’s rotation. 
The coordinate frame to be used is the North-East-Up, with the origin O u generally at an 

arbitrary ground point, here chosen to be at the quadrotor take-off point. As shown in 
figure 32, the unit vector ‘x’ points toward North, the unit vector V toward East and ‘z’ 
toward the opposite direction of the center of the Earth. 



Figure 31. Earth Fixed Inertial Frame U 
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b. Body- Fixed Frame B 

The body frame is rigidly attached to and defined within the vehicle and 
exists to specify quadrotor’s orientation. The definition for this frame has the x-axis along 
the two opposing propellers axis, the y axis pointed along the other two opposing 
propellers and the z-axis upward, forming the right hand set. This frame is shown below 
in figure 33 and Roll, Pitch and Yaw are defined as the angles of rotation tp, 0 and v|/ 
about x, y and z axis, respectively. The origin of the body frame is at the center of the 
mass of the quadrotor. 


Origin O b 
(CM) 

Figure 32. Body-fixed Frame B 

2. Modeling Assumptions 

At this point several reasonable assumptions concerning the modeling of the 
quadrotor must also be made: 

• The Earth is flat, non-rotating, and an approximate inertial reference 
frame. 

• The acceleration of gravity is constant and perpendicular to the surface of 
the Earth. 

• The design is symmetrical with respect to the axis. 

• The quadrotor body as well as the propellers will be treated as rigid body, 
so that the Newton- Euler Formulas can be used. 

• Since, it is an indoor experiment, and the speed is considered to be low, 
the air friction will be ignored, gyroscopic effects and the aerodynamic 
torques can be cancelled. 

• No swash plate exists for each rotor. 
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3. 


Vehicle State Variables 


The quadrotor QBALL X-4 is an unmanned helicopter with four rotors combined 
with a cross scheme. The quadrotor generates its lift by these four rotors. The two 
opposing rotors form one pair, where the first pair (#1 and #3) is set on the x-axis and is 
rotated counter clockwise while the second one (#2 and #4) is set on the y-axis, rotated 
counterclockwise as shown in Figure 34. 


Pitch Roll 



Figure 33. Quadrotor Configuration Scheme 

The earth-fixed inertial frame (Xu, Yu, Zu ) shown, specify the location of the 
vehicle, while the body frame (Xb, Yb, Zb) specify the vehicle orientation, together with 
angles of rotation, roll (cjY), pith (9), and yaw (y/). 

Let the mass of the quadrotor be m that represents the whole structure mass of the 
quadrotor, as it stated in the assumption (d) in the previous section. 
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Let B V the velocity vector of the vehicle and 11 co the rate of change of the angle 
(angular velocity) in body frame, respectively, u x the position of the vehicle in the 
inertial system 


3 V = ui + vj + wk = 


3 co = pi + qj + rk = 


“x = xi + yj + zk = 


u 

V 

w 

p 

q 

r 

x 

y 

z 


( 1 ) 


( 2 ) 


( 3 ) 


Also, the Euler angles of the vehicle which rotates around the XYZ Body frame 
axis are represented as 


A = 


<P 

9 

¥ 


( 4 ) 


4. Transformation Matrices 


In order to transfonn the state vectors from {U} frame to the {B} frame, a 
transfonnation matrix must be used. This matrix according to the convention of rotate 
first around x-axis, then around y axis and finally by the z axis as described in [1] can be 
found by the following equation: 


B 

u 


R = R 


~X,(j 


'-R. 


: R 


z,y/ 


( 5 ) 


where R X ^,R V0 ,R ZV , are the rotation matrices around each axis, respectively, as 
follows: 
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Rotation around x-axis: R x ^ = 


10 0 
0 cos (f) sin (f) 
0 -sin^ cos (f) 


( 6 ) 


with yaw angle yr < 


f 71 7T^ 


2 2 


Rotation around y-axis: R v g = 


cos# 

0 

-sin# 

0 

1 

0 

sin# 

0 

cos# 


( 7 ) 


with pitch angle 9 • 


f 71 7T^ 


2 2 


Rotation around z-axis: R z = 


cos^ sin^ 0 
-sin^ cos^ 0 
0 0 1 


( 8 ) 


with roll angle (j) < 


f 71 7T^ 

2 ’ 2 


So, if we use the Eq. 6, 7 and 8 in Eq. 3.5, the final form of the transformation 
matrix can be derived: 


B uR = K,t*K.e*R:, ¥ = 


cosOcosi// 

sin (j) sin 6 cos y/ - cos (f) sin y/ 
cos (j) sin 0 cos y/ + sin (j) sin yr 


sin yr cos 6 

- sin (j) sin 6 sin yr + cos 0 cos yr 
cos (j) sin 9 sin y/ - sin (j) cos y/ 


-sin 9 
sin (f> cos 9 
cos (j) cos 9 


( 9 ) 


The transformation matrix from {B} to the {U} coordinate frame is: 


u 

B 


R = 


cos 9 cos y/ 
cos 9 sin y/ 
sin# 


- cos (j) sin yr + sin (j) sin 9 sin yr 
cos (j) cos y/ + sin (j) sin 9 sin yr 
sin (j) cos# 


sincj) sin y/ + cos (j) sin # cos yr 
- sin (jr cos yr + cos 0 sin # sin yr 
cos (j) cos 9 


( 10 ) 
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5. Kinematic Equations 

Having defined the transformation matrices, we can now find the velocity of the 
vehicle in the inertial frame instead of the Body coordinate frame and it can be given by: 



X 


u 

“x = 

y 

= l ‘R B V= “R 

V 


z 


w 


(cos 6 cos y/)u + (- cos cp sin y/ + sin cp sin 6 sin y/)v + (sirup sin yr + cos cp sin 0 cos y/)w 
(cos 6 sin y/)u + (cos cp cos y/ + sin cp sin 0 sin y/)v + (- sin cp cos y/ + cos (p sin 0 sin y/)w 
(sin 0)u + (sin cp cos 6)v + (cos cp cos 6)w 


(3.11) 


Moreover, the rate of change of the Euler angles of the vehicle as stated in [1] can 
be found by: 



A 


e 

v 


Q\K) b (d 


( 12 ) 


where the matrix Q(A) and its inverse one, Q '(A) are given by the equation: 


0(A) = 


1 

0 

0 


0 -sin <9 
cos (f> sin </> cos 6 
-sin^ cos ^ cos 6 


Q\ A) 


1 sin (j) tan 6 cos (f) tan 6 

0 cos (f> -sin^ 

0 sin (f) / cos 6 cos ^ / cos 6 


(13) 


(14) 


So, using the Eq. 2 in Eq. 12 the following equation can be derived: 




“i 

sin (f> tan 6 

cos (f) tan 6 

P 


e 

= 

0 

COS (f) 

-siruf) 

q 

(15) 

y\ 


0 

sin^/ cos 9 

cos (f)! cos 0 

r 



The last equation and Eq. 11 are the kinematic equations of the quadrotor. 
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6. Dynamic Equations 

According to Newton’s second law in the {U} orientation frame, the following 
equation represents the applied force U F and the acceleration of the vehicle u x in the 
{U} frame: 



1 

EC 

1_ 

t 

X 

1 

_1 

= —(m V) = m — V = m x = m 
dt dt 

1 

i_ 


(16) 


If we multiply by both sides the transformation matrix B u R of Eq. 9, Eq. 16 will be 


transfonned to: 


V b R u F = y b F = B Rm“x = m B R 

u u u 


(17) 


=>1 


B F=m B R — { u x ) 
“ dt 


(18) 


Continuing the math: 

!L‘ F =”l R ^C,R‘V) = mlRC,R‘V+‘ B R‘V) = m( , 'R',R , V+lR- B R‘V)=> 

y b F =m( B V+ B R ‘‘R b V) = m( B F+ B R[^RS( B a))]x B V) => 

Y J B F = m( B V + S( B co)x B V)) (19) 


Since B R = “RS( B a>), where S( B co) is a skew symmetric matrix. The term 
S( B co)x B V is the Coriolis term and can be written as: 


S{ B co)xV B = B d>xV B 


qw-rv 
ru - pw 
pv - qu 


( 20 ) 
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If we substitute the derivative of Eq. 1 and the Eq. 20 into Eq. 19 and rearrange 
the terms, then: 



u 


qw-rv 

)=>- 

1 

_1 


u 


qw-rv 

Y,‘F = m( 

V 

+ 

ru - pw 

F y' 

= 

V 

+ 

ru - pw 

w 


pv - qu 

m 

F A 


w 


pv - qu 



=> 

u 

V 

1 

i_ 


qw-rv 

ru - pw 


w 

m 

F z \ 


pv - qu 


( 21 ) 


where F x ,F v ,F_ are the forces in the body coordinate frame in each axis, u,v,w 
and u,v,w are the velocity and the acceleration components in the body frame, 
respectively, in each axis and p,q,r are the angular velocity components in the body 
frame in each axis. 


Using the Euler’s Law, the momentum U M 0 in the inertia frame will be given by: 
V “M 0 = — ( U A) = — ( u b RJ b B a>) = 


B RJ b b g) + B RJ B b g> + Mfcb = “R( B d)xJ B B a>) + ;RJ b B d> ■. 


x b r "M 0 = B u R[;R{ B d>xj B B d»]+ B U R[ ;rj b b &\ ■. 




L 

M 

N 


= ( B d)xJ B B d>) + J B B cb 


( 22 ) 


where L, M, and N rolling, pitching and yawing moments, respectively, in the 
body coordinate system, B A the angular momentum in the Body frame and J B is the 
Inertia Tensor of the quadrotor in the B coordination frame ( J B e R 3x3 > 0): 


J B = 


f J J J A 

xx xy xz 
Jyz Jyy Jyz 

.J J J . 

y zt zy zz J 


(23) 
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Then, the angular acceleration will be given by the equation: 


* & = V E B M 0 -( b G)xJ b B a>)] (24) 

0 

0 

J_ 

z 

since J = J. _ = J x _ = J_ x = J v = J _ r = 0 because the quadrotor is symmetric about all 
three axes. 

If we take the derivative of the angular velocity in Eq. 2: 



And combine Eq. 2 with Eq. 23 to take the external product, we will have: 

P\ J , X P- J X y ( i~ J X / 

B a>xJ B B a>= q x -J yx p + J yy q - J y / = 

r -J a P-Jy<l + J B r 

' (27) 

~ r (- J y X P + J uP ~ J yS) + qi~J a P ~ J zy<l + J zz r ) 

r ( J xx P - J XX P ~ J„r) ~ P(~J-_ X P - J zy q + Jjr ) 

~q(J xx P ~ J xy q - J X S) + P(- J y X P + JyyP ~ J yS) 

Finally, if we substitute Eq. 3.31 and Eq.3.33 in equation 3.30 we will have: 

P~\ I" L 1 ~ r (~Jy X P + J yy C l ~ J y/) + C l(~ J z X P ~ J .PI + J zz V ) 

B a)= q =J-\M - r(J xx p - J xy q - J xz r) - p(-J zx p - J zy q + J z r) )=> 

j J [_ N J L -VVxxP ~ + P(- J y X P + J yPl ~ J y/) _ 
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( 28 ) 





The last equation together and the Eq. 21 are the dynamic equations of the 
quadrotor. Combining the kinematic and the dynamic equations of the quadrotor you end 
up with the nonlinear differential equations of motion of a rigid body with six degrees of 
freedom. 


7. Forces Calculation 

a. Gravity Force 

Gravity F G is the main force acting on the quadrotor in the earth fixed 

frame along z-axis toward the ground, which is given by the total mass of the quadrotor 
multiplied by the acceleration of the gravity as follows: 

“ 0 " 

“F g = 0 (29) 

mg 

where m is the total mass of the vehicle, g is the acceleration of the gravity. 
Consequently, Eq. 29 will be transformed in the body coordination frame using the 
transformation matrix B u R from Eq. 9: 

0 -mg sin 6 F Gx 

b F g = b u R 0 = mg sin cp cos 6 = F Gy (30) 

mg mg cos cp cos 6 F Gz 

where 11 F c is the gravitational force in the body frame and F G1 , F G2 , F G3 are the 
components of the gravity force in the x, y , and z direction, respectively. 
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b. Drag Force 

The second force acting on the quadrotor is the drag force that opposes the 
movement of the quadrotor and is equal to the coefficient of drag multiplied by the 
square of the velocity in every axis. In the inertial frame the drag force is given by the 

\c d Ap' v < 

equation: "F 0 = ^ C llr A r p‘V; (31) 

\c d ap'K 2 

where A is the cross-sectional area, p is the air density, V x , V v , V . are the body frame 
velocity components in the x, y, and z axis, respectively. C Dx , C Dy , C Dz are the drag 
coefficients in each axis. In order to get the drag force components F Dx ,F Dy ,F Dz in the x, 
y, and z direction in the body frame, we will also use the matrix^ from Eq. 9. 

] -C, )x A r px 2 

\c d Ap? 

\CnApi 1 

However, the velocity regime in which the quadrotor operates is at slow 
airspeeds. Avoiding the complex calculation of computational fluid dynamics and using 
the same considerations as in [1], where the coefficients of drag varies with the object’s 
shape about ( C Dx = C Dy = C D _ =1.12) and the cross-sectional area facing each axis and 

the combined surface area of the cross-frame with batteries and motors, are considered, 
then A c is found to be about 0.16m for the x and y axis and 0.4m for the z-axis, 

respectively. Also, by using as the common air density value of 977.2 Kg / m , the 
resulting drag force components are found to be around 
F Dx = 8.756*10 5 *i 2 Kg / m,F Dy = 8.756 * 10 5 *yKg / m,F D: = 5.289* 1O^ 4 * z 2 Kg / m . 
This is quite negligible and thus can be ignored. 
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c. Thrust forces 

The quadrotor’s motion is only controlled by varying independently the 
speed of the four rotors. Each rotor produces both a thrust and a torque (moment) about 
its center of rotation. Let F T . be the thrust for its rotor, respectively, and / the distance 

of each rotor from the center of the quadrotor’s mass. The quadrotor’s motion is affected 
by the four propellers only in the z-direction so we have that: 



0 


1 

_1 

II 

0 

= 

Fry 


Ft i "*■ F T 2 + Fj^ + F T4 \ 


1 

_ 1 


(33) 


where F n ,F T2 ,F T3 ,F T4 are the thrusts that the 1 st , 2 nd , 3 rd and 4 th rotor produces, 
respectively, and F lx , F lv , F r _ are the total thrust force in each direction in body frame. 


d. Total Force 


The total force is the sum of the gravity, drag and thrust forces: 


Z‘ F = 


p 4-/7 4 - F 

1 Dx T 1 Tx T 1 Gx 

P + p + p 

r Dv ^ r Ty ^ r Gy 

P + p + p 

1 Dz^ 1 Tz^ 1 Gz . 




\c d Apx 2 



-mg sin 0 


1 

{c^Ap.v 1 


0 

mg sin (p cos 6 

+ 

+ 

0 

mg cos cp cos 6 




F Tl + F r 2 + F t 3 + F t4 _ 


F x 

F y 

F_ 


1 

~mg sin 6* + — C Dx A c px~ 


1 


• 2 


mg sin (p cos $ + ~ C Dy A c py 

1 

mg cos (p cos d + — C Dz A c pz + (F n + F T2 + F T2 + F T4 ) 


(34) 
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8. Moments (Torques) Calculation 

Let r t be the torque generated by each propeller. The rolling moment L(r ro/; ) is 
produced by varying the right (#2) and the left (#4) rotor speeds 

L = ^ roii =r 2 -r 4 = F r l- F T l = (F 7 . - F Ti )l = (F right - F left )l 

Similarly the pitching moment M(j pitch ) (torque) is produced by varying the front 
(#1) and the back rotor (#3) speeds 

M = r pitch =r, -r 3 = F r l- F T l = (F 7 . - F T> )l = (F frort - F hack )l 

Due to the third Newton’s law, the drag of the propellers produces a yawing 
moment (torque) on the body of the quadrotor. Therefore, the total yawing moment is 
obtained from all the rotor speeds (clockwise and counterclockwise), is in opposite 
direction of the motion of the propellers and is given by 

N = r yaw = {F Ti + F T} - F Ti - F Ti )d = (F right + F lefi - F fmnt - F back )d 

where d is the force to moment scaling factor calculated to be d=4N.m. 

9. Moments of Inertia Calculations 

a. Moment of Inertia along x 

If the two motors (#1 and #3) along x axis and the main, central body of 
the quadrotor considered cylindrical in shape with mass m t , m 3 and m , radius 
R t , R 3 and R and height/?,,/?., and h , respectively, as shown in Figure 34. 


z-axis 



Figure 34. Moments of inertia about x, y and z-axis. 
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Only the rolling motion is considered. So, the moment of inertia about the 
x axis would be due to the main body and the motors #1 and #3 motion about x-axis and 
due the motion of the other two motors (#2 and #4) about x axis 

= J c + J\ + + j ia (38) 


The moment of inertia of a cylinder about an axis perpendicular to its 
body, as specified in Halliday- Resnick-Walker [57], thus the moment of inertia due to 
main body and the two motors are 


j _ m c * r c 2 ! m c * K 2 

4 12 


T m,*r , 2 m,*h , 2 

J x =——- + —-— 

4 12 


m,*r~ m ,* hr 
J ,+ ■ 3 3 


12 


(39) 

(40) 

(41) 


The moment of inertia of two identical spheres connected together by a 
horizontal arm, and rotating about a vertical axis, which is passing through the center of 
the ann and is perpendicular to it, as also specified in [57], thus the moment of inertia 
due to motors #2 and #4 rotating about x-axis approximated to be 

J 2 - 4 ,x=2m r l 2 ( 42 ) 


Since the motors are identical: 


znj = m, = m 2 = m 4 = m r , i\ = r 3 = r 2 = r 4 = r and \ = h 3 =h 2 = h 4 = h r 

Finally, the total inertia about x axis by substituting Eq. 39 to 42 in Eq. 38 


is 


mr 2 mh 2 mr 2 m h 2 l2 
J„ = —— + —— + ° c + ° c + 2 ml 


12 


(43) 
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b. Moment of Inertia along x 

Similarly, because the quadrotor structure is symmetrical and the motors 
are identical the moment of inertia about y-axis would be exactly the same: 


J 


yy 



mh 2 m r 2 m h 2 

_ r _|_ o c _|_ o c 

6 4 12 


+ 2 m r l 2 


(44) 


c. Moment of Inertia about z-axis 

The moment of inertia about z-axis, will be due to main body and due to 
all the motors. The moment of inertia due to the main body is found in [57] to be: 


J 


c,z 


m c r c 

2 


(45) 


The moment of inertia due to all the motors #1 to #4 is approximated to 
be: 




l-2-3-4,z 


= 4m,./ 2 


(46) 


Therefore, the total inertia about z-axis is: 


J = 


mr 


■ + 4m Jz 


(47) 


All the above moments of inertia were calculated for the quadrotor and 
found to be as follows: 


J xx = 0.03 Kgm 2 

J yy = 0.03 Kgm 2 . (48) 

J.. = 0.04 Kgm 2 
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10. Final Equations of Motion 

Combining the Eq.34 with the dynamic Eq. 21 we can get: 





1 2 

-mg sm 9+ -C D< A c px 




u 

1 

m 

Z 

1 2 

mg sm (pcos9 + -C Dy A c py 


qw - rv 

=> 

V 

. 

- 

ru - pw 


w 


1 

mg cos (pco%9 + -C D .A c pz + (F r _ + F Ti + F r + F Ti ) 


pv - qu 


u 

v 

w 


1 

2m 

1 

2m 


-g sin 9 + — C Dx A c px 2 + (rv - qw) 


g sin <p cos 9 + —— C D; .A c py 2 + (pw -ru) 


1 t 1 

g cos (p cos 0 +- C D ,A c pz~ + — (F r +F r +F T + F T ) + qu- pv 

2m “ m 1 2 3 4 ■ 


(49) 


If we neglect the drag force then the equation takes the fonn: 


u 

v 

w 


-g sin 9 + (rv - qw) 
gsin^cos# + ( pw - ru ) 

g cos cp cos 9 + — (F r +F t +F t +F T ) + qu- pv 

i/T/1 12 3 4 


(50) 


Similarly as before, if Eq. 17 is combined with Eq.34 and the transformation 
matrix in Eq. 9 is applied, then: 
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T' F 


1 

1 _ 


X 


X 


1 

1 _ 

F A 

= B u Rm 

y 


y 

Fy 





m 

FA 


z 


z 


F_ 



P 

q 

r 


(52) 


y 


— (F T +F t +F t + F t )(cos^sin^ r + cos^sin^cos^) 

m 1 2 3 4 

— (F t +F t +F t + F t )(-sin^cos^+ cos^sin^sin^ r ) 

m 1 2 3 4 

— (F r +F t +F t +F t ) cos </> cos 0-g 

m 1 2 3 4 


(51) 


Substituting Eqs. 35, 36 and 37 in Eq. 28 becomes 

t 2 - t 4 qr 


J j (S. J yy) 

XX XX 

r -^-PL{j -J ) 

J J v XX zz J 

J yy J yy 


(F Ti + F T} F Ti F T4 )cl pq 

J J ^ ^ xx 


( F t 2 Ft 4 )/ qr 

J. J, 1 zz ^ 


(F t ~ F t )/ n/" 

— ■ -jf-GW.) 

J yy J yy 


(F Tt + F- h F h F Ti )d pq 

J J. ^ xx) 


Equations 15, 51 and 52 form the dynamical model of the quadrotor that will be 
used for the design of the controller. Although some approximations and assumptions 
already were made, some more simplifications have to be done for the better 
implementation of the control scheme. 
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B. MODELLING OF GROUND ROBOT QBOT 


1. Differential Drive Kinematics 


The notation and a mathematical model for the kinematics of a wheeled mobile 
robot will be developed in this section complying with references [56]-[61 ]. Wheeled 
robots have some simplifying features that make them easier to be modeled than real 
vehicles, since the operation field is on 2-D space environment. In particular, these robots 
have two independently driven, coaxial wheels. The speed difference between the two 
wheels causes a rotation of the robot about the center of the axis while the wheels 
produce motion in the forward or reverse direction. It is known that real vehicle 
dynamics, at high velocities, cause a vertical motion which is compensated for by 
applying suspension to their design. However, because these robots operate in very low 
speeds the vertical motion is almost negligible and no suspension is required. 

Since the robot is moving on a horizontal plane, the degrees of freedom are three 
(3 DOF), the x and y positions along the axis of the differential drive and the rotation 
around z-axis: 



(53) 


The following figure 34 depicts the kinematics of the iRobot Create, where the 
distance between the two wheels’ centers is L=0.34m. 


i 



-> X 


Figure 35. Kinematics of the differential drive robot, iRobot Create. 
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As described before, in the case of the quadrotor, the motion has to be 
transfonned from the body (robot here) frame to the inertial frame. The transformation 
matrix of Eq.9 will be “rotation around the z-axis” since we are dealing only with a 2-D 
frame: 


u 

B 


R, = 


cos.9 

-sini9 

0 


sin 5 
cos 9 
0 


0 

0 

1 


So, Eq.53 becomes: 



u x 


cos^ 

sin^ 

0" 

X 

“P = l ‘R z B P => 

u 

y 

= 

-sin^ 

cos^ 

0 

y 


u 9 


0 

0 

1 

9 


"“xl 


xcos<9+ vsin^ 

u y\ 

= 

-xsin^ + y cos^ 

u 9 


9 


x = xcoS (9 + ysini 9 
y = -xsin, 9 +vcos .9 


u 9 = 9 


(54) 


If the position of its wheels’ centers areC ; , / = 1,2 for right and left 
wheel, respectively, the longitudinal component of velocity is v longi and in lateral 
direction is v lati , by differentiating Eq. 53the velocity of C ( can be obtained in the body 
frame (2x1 vector): 


B 


B • 

v long,i 


X 

B 


B • 

lat,i 


L T J 


( 55 ) 
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From body frame to the inertia frame: 


u 


X = 


X 


y 


;rv=;« 


u 

V 


u 

v long,i 


X 

1 

a 

3 

_1 


_y_ 


cos 3 
-sint9 
0 


sini9 
cos & 
0 


0 

0 

1 


v long, i 


lat.i 


u 

^ long,i 

II 

-1 

u 

L V lat,i \ 

- 

l 

£ 

A 

© 

3 

_1 

II 

1 

1 

3 

3 

_1 



' V lon gJ C0S & + \t,i S ' n 3 
B, 


-~ V lon g ,i Sin $+ V ta, :l C° S & 


x cos <9 + v sin 3 
-jcsin<9 +j> cos <9 


(56) 


Consider the rolling wheel with a point of contact P with the ground. 



Figure 36. Top View of the Robot 

Because it is rolling, the velocity of point P is zero. Suppose the wheel 
rolls along the y axis on the y-z plane given by x = 0. If we recall from physics [57] that 
the velocities of two points A and B on a rigid body with angular velocity go, are related 
by the equation: 

v B =v A +coxAB 
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Then the velocity of the center C i can be obtained from the previous 


equation: 

v CJ = v P + eo i x PCi (57) 

If (p n i = 1,2 each wheel’s speed, respectively, then the velocity of the center C ; 
will be ( i, j, k the unit vectors along x, y, z axis, respectively): 

v c,i= ( °i xr i= ( PJ x rk = -r<pj (58) 

Since the robot cannot slide in a lateral direction, the velocity of the point 
C i must be along the longitudinal, thus the velocity of the point C ( in the lateral direction 
must be zero: 

v lat . = 0 => -Jc j sin <9 + y, cos <9 = 0 
After rearranging terms: 

x t sin <9 = j>. cos <9 x i tan <9 = y ; . (59) 

Then the Eq. 65 will be equal to: 

V lon g ,i = ~ r <Pi ( 6 °) 

V lat,i = 0 

Next, (p t is measured in a counterclockwise direction from one arbitrarily 
chosen side of the robot as 



Figure 37. The wheel angles (Positive in a counterclockwisedirection from one 
(arbitrarily chosen) side of the mobile robot). After [58]. 
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Now consider the coordinates of the center of the axle (x, y) which is 
clearlyhalf way between C, and C 2 : 


x, + x, 
x = —-- 

2 (61) 
v=A±2i 
2 

The velocity of the point C i is given by: 


v c,/ = 


Xj +x 2 
2 

T1+T2 


(62) 


The longitudinal velocity component if we combine Eq.56, 60 and 62will 
become: 


v bn g j =xcos$+ysin$: 


x, cos <9 + sin <9 x 2 cos <9 + y 2 sin <9 _ V C, 1 + V < 


■ + 


c, 2 


r<Pi+r<p 2 


v long, i 


(63) 


Now if we consider the two points Q and C 2 which are rigidly 

attached to theaxle and the robot, like Eq. 57, the velocities of these two points are related 
by theequation: 

v c 2 = v c, +dkxC ] C 2 (64) 

That can be transformed to: 


—r<p 2 = -rcp x + Zi9 LS = rtp x - r<p 2 ^>9= 

L 

If the velocities of the left and right wheel are denoted as: 

% = <Pri S h, anCl Vl = (ftleft 


V C, = V right mCl 


V C 2 =Vuft 


( 65 ) 
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we have the two equations that relate the robot velocities tothe the wheels’ speeds: 


v 


long,i 


bright +r< P,eft 

2 


( 66 ) 


n_ r( Pright -m eft 

L 

If we substitute Eq.59into Eq.57: 


(67) 


v long, i 


, = x cos i9 + Jc tan & sin & = i(cos & + 


sin" & 
cos (9 


) = *(- 


cos <9 


X= V ,on g ,i COS ' 9 


( 68 ) 


Also sin ce y = x tan ,9 = v u . 


sin 9 


y = V long,i 


sin .9 


(69) 


Finally, if we substitute the Eq.63 into the state equations of Eq.54, they can be written in 
the inertia frame as: 


P = 



V ■ , + V, <■ 

nghr le ^ cos i9 
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1_ 


r r 
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70) 


The velocities of the wheels are bounded such that: 


—v < V, , < V 

max left max 


and —v < v , < v 

K max v right K max 


(71) 


where = 0.5 mls 

The robot will move forward when v rjght > v le/t >0, left when v left > v right >0 and 
will spin in place when v lef - v ri ht , and not equal to zero. 
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Returning to the definition of a two wheel differential drive robot, it has two drive 
wheels mounted on a common axis and each wheel is controlled by a different motor. 
Therefore, each wheel is able to be driven at different velocities either forward or 
backward. As it is already reported before, by varying the velocity of each wheel, rolling 
motion can be achieved and the robot will rotate about a point that lies along the common 
left and right wheel axis. This point is widely known as the ICC - Instantaneous Center of 
Curvature [58] (See Figure 37). 



v, x- 


L 


12 


R 


[*. y) 



Figure 38. Kinematics of differential robots. After [58], 

The trajectory of the robot can be controlled by varying the velocities of the 
wheels. The rate of rotation co about the ICC must be the same for both wheels. Hence, 
the following equations establish a relation between the motion parameters of a 
differential drive mobile robot. 

>W,=®(* + f> (72) 

(73) 

where L is the distance between the centers of the two wheels, v right ,v left are the right and 

left wheel velocities along the ground , and R is the signed distance from the ICC to the 
midpoint between the wheels. 
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The Eq.72 and 73 can be solved at any instance of time for R and co as follows: 


bright + V left) 
2 0 right - V left) 


(74) 


V , —V,r 

right left 

co = — -- 

L 


(75) 


Figure 37 assumes that the robot is at some position (x, y) and is heading in a 
direction making an angle 3 with the X axis. Knowing velocity v right ,v left , and using 

Eq.74, 75, the ICC location ((ICC x ,ICC y ) is determined as: 


ICC X 


ICC v 

y 


x - R sin 3 = x-^ ng/ "-sin 3 

2 ( V right 

(76) 

v + Rcos3= v + ^ " gl " — —cos3 

2 ( V right ~ V lef,) 

(77) 


2. Forward Kinematics 

The forward kinematics problem of a mobile robot for given wheel velocities and 
initial robot's configuration (x, y, 3) t=0 is the determination of the robot's position 

(x, y, <9), at any other time t [56]. 


a. Robot's Position Relative to ICC Location 

If non-varying velocities v nghl , v h , ft are given, the ICC location 
(ICC x ,ICC v ) will be a fixed position. Hence, the robot’s state at time t + At can be 
expressed via its state at time t as follows: 


x’~ 


cos (coxSt) 

-sin(®xc)t) 

0 “ 

~x-ICC ~ 


ice; 

y' 

= 

sin (cox St) 

cos (cox St) 

0 

y-icc y 

+ 

ICC y 

3' 


0 

0 

1 

3 


coxSt 
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where (x,y,9) and (x \ v<9') are the robot's positions at time t and t + At, respectively. 
This equation describes the motion of a robot rotating about its ICC with a radius of 
curvature R and an angular velocity co (see Figure 40). 

P(t+*t) 


4 p «) 


Figure 39. Forward kinematics relative to ICC. After [58]. 

b. Robot's Position Relative to its Initial Position 

General motion equations of the robot capable of moving is in a particular 
direction 9(t) at a given velocity v(t) are described as follows: 

t 

x(t) = | v(/)cos[i 9(t)]dt 
0 
t 

y(t ) = | v(t) sm[9(t)\dt (79) 

0 
t 

9(t) = J co{t)dt 

o 

For a differential drive robot, such as the Qbot, Eq. 79 becomes: 

1 f 

x(t) = ~ J [v right (0 + v left (0] cos[9{t)]dt 

y(t) = [v righ) (t) + v ¥t (t)]sm[9(t)]dt (80) 

3(t) = j\[v righl (t)-V l e ft (t)]dt 
L 0 

If the Eq.79 is simplified, the robot's position at time t + At will be: 
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x' x + vcos[i9(t)]f>i 

y' = y+ vsin[i9(t)]c>t (81) 

&' 1 9 + coSt 

Similarly, the Eq. 80 will become: 

^ t ^right (t) + v left (t)\cos[3(t)}St 

y + |[v right (t ) + v left (i)> in [3{t)\8t (82) 

’ 9 + j[ V n g h t ( t )-Vie fl (t)]3t 

For the special cases of v right = v lefl = v and -v right = v left = v in Eq.81, we 

will have 

x + vcosi9f>t 

y + vsin^t (83) 

x 

y , (84) 

i9 + —2 vSt 

L J 

respectively. 
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C. CONTROL ARCHITECTURE OF THE TWO MODELS 

Now that kinematics / dynamics for both vehicles have been developed, we can 
elaborate on Figures 30 and 31 and present overall feedback control architecture (Figure 
41). 
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Figure 40. Control Architecture 


Figures 42 and 43 provide more details about the robot plant models and explain 
graphically the derivation of the equations of motion and how it will be implemented in 
the Simulink development environment. 
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Figure 41. Quadrotor QBall-X4 Dynamics Plant 
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QBOT KINEMATICS MODEL 
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Figure 42. Ground Robot Qbot Kinematics Representation 
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IV. CONTROL STRATEGY 


A. INTRODUCTION 

The dynamic model of the quadrotor has been presented in many papers, so with 
some small variations, concerning the different assumptions to be done or the 
environment, it will be the same. However, the control schemes can be different, 
choosing from feedback linearization control, backstepping control, visual control and 
direct methods with inverse dynamics. In this chapter, the general control scheme will be 
presented. The first controller is the implemented one with the Linear Quadratic 
Regulator method imposed in five different channels, roll, pitch, x-y position, yaw and 
height. Although, the modeling took place for a nonlinear model, the nature of the 
experiments (laboratory - slow speeds) allows us to use a linearized model for the 
quadrotor control. Secondly, another controller is proposed using inverse dynamics in 
Virtual domain (IDVD) control technique for achieving a quasi-optimal solution in real 
time (tenths of second). This controller requires creating a trajectory generator with 
certain characteristics that will be described below and follows the quadrotor dynamics. 
Unfortunately, this controller was not implemented and tested for this model but it 
provides the steps and so far implementation for future work within the students already 
or starting working in this project. 

B. IMPLEMENTED CONTROLLERS 

1. Control Inputs 

The quadrotor is controlled by independently varying the speed of the four rotors. 
For every attitude change the angular velocity of motors is changed, but the total thrust of 
all the four motors is constant in order to maintain the height. So, in order to produce a 
roll angle (</> ) along the axis X B ,the angular velocity of the motor #2 is increased and the 
angular velocity of motor #4 must be decreased. Likewise, in order to produce a pitch 
angle (0) along the axis Y B , the angular velocity of the motors #3 must be increased, and 
the angular velocity of motor #1 must be decreased while at the same time the thrust 
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must be kept constant. It can be understood that yaw motion along the Z-axis of the body 
frame will be implemented by increasing total angular velocity of motors (1, 3) and 
decreasing the angular velocity of opposite rotation motors (2, 4). The rotors of quadrotor 
are located six inches from the end point of the aluminum rods and l is the length 
between the rotational axis of each rotor and the center of gravity of the quadrotor. So we 
can assign the following control inputs, U i : 


U,= 


Fj + F t + Fj + Fj 


m 


U 2 =F Ti -F T4 
U 3 =F t -Fj. 


U 4 = ( F t + F t - F t - F t )d 


(85) 


where F Tj and d are, respectively, the nonnalized thrust force from each (ith ) rotor and 

d is the force to moment scaling factor coefficient depending on the blade’s Reynolds 
Number, Mach number and angle of attack. 


2. Waypoint Navigation 

Waypoint navigation for the quadrotor will take place in two different ways. The 
Qball-X4 quadrotor will follow the position of the Qbot, which acts as the leader. 

a. Qbot’s waypoint navigation via the Qbot model, an autonomous 
waypoint navigation program that moves Qbot through a series of predetennined 
waypoints chosen by the user. 

b. The quadrotor navigates by following the waypoints taken from 
the Qbot through the stream client of the real-time workshop. So, Qbot’s Tx, Ty 
coordinates, are taken as inputs so a waypoint state machine can be built with the 
following states: 

• Initialize 

• Takeoff 

• Follow waypoint 

• Land 
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The waypoint state machine will allow quadrotor to follow the 
Qbot’s position through its controllers. Alternatively, a trajectory generator with inverse 
dynamics can be used to generate new quasi-optimal trajectories for following the 
waypoints. 


3. Linearization of Qball-X4 Control 

Since the drag forces are negligible and the angles of the orientations are very 
small for hover flights [1], small angle approximation are invoiced: 


cj) <§; 0.1 => sin(^) = 0, cos(^) = 1 
0 <sc 0.1 => sin(60 = 0, cos(60 = 1 


( 86 ) 


So the kinematic Eq. 15, can be simplified to: 


(t> 


'i 
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0 " 

-p 
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0 

1 

0 

q 

y\ 


0 

0 

1 

r 


(87) 


If we take the derivatives of the previous equations and apply Eq.28 , then: 


# = p = Ul_1L (J _j } 

Y t' j j \ zz yy) 

XX XX 

J yy J yy 


.. . U 4 pq 

u/ = r = —- — -J ) 

T j -j V yy XX ' 

u ZZ ^ ZZ 


( 88 ) 


If Coriolis term is ignored, the simplified equations of motions can be derived as: 
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<i = P 


U 2 l 

^XX 


4 


¥ 



Eq. 56 can be written with the controls as follows: 


X 


U x (- sin (j) sin yr + cos (j) sin 9 cos yr ) 

y 

= 

U x (-sin (j) cos ^ + cos ^ sin 0 sin ^) 

z 


U x cos (j) cos 9-g 


( 89 ) 


(90) 


Hence, a simplified model can be presented with the state equation, that can be 
used for both controllers, the implemented and the proposed one: 

r x 1 


X 


y 

z 

X 


y 


x = 


d_ 

dt 


$ 


6 

¥ 

<t> 

9 

¥ 


y 

z 

U x (cos (f> sin y/ + cos (j) sin 6 cos y/) 
U x (- sin (f) cos y/ + cos ^ sin 6 sin y/) 
U x cos ^ cos 6 - g 

</> 

9 

¥ 

U£ 

JXX 

U 3 l 

Ul 

•4 


f(X,U) 


(91) 


where X = [x, y, z, u,v, w, (j), 9, y/, p, q, rf = [x, y, z, x, y, z, (j), 9, y/,(j),9,y/^ 


(92) 


and 


U = [U x ,U 2 ,U 3 ,U 4 f 


(93) 
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4. LQR Controllers / Channels Description 

a. Rotor (Actuatorj Dynamics 

The thrust that is generated by each propeller is modeled by the first order 

system equation: F T t = K — u t (93) 

s + co 

where u i is the PWM input of the actuator, co is the actuator bandwidth and K is a positive 

gain. Those parameters are already calculated through experimental studies in Quanser 
[51], and they are given in Table 7. 


Parameter 

Explanation 

Value 

Unit 

K 

PWM input 

of the actuator 

120 

N 

CD 

Actuator Bandwidth 

15 

rad/sec 


Table 7. Thrust parameters 


A state variable v t will be used to represent actuator 
dynamics, which is defined as follows: 

v,=K—u, (94) 

S + O) 

b. Roll and Pitch Models 

Assuming that rotations about the x and y axes are decoupled, two 
propellers contribute to roll/pitch axis already modeled and discussed in the previous 
chapter. The rotation around the center of gravity is produced by the difference in the 
generated thrusts. If we set A u the same for pitch and roll, the moments of inertia are 

found to be the same ^ xx ^ yv ^ since the quadrotor already considered symmetric, the 
models can be formulated as also stated in [51]: 
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v . U 2 l l co 

<p = p = ^^ = —K -A u 

J xx *4 s ' ® 

■A . uj l CO 

0 = q = -=- K - An 

J yy J yy S + 0) 


Aii =u l - u 3 or Au = u 2 - n 4 


If we combine these last three equations and the actuator dynamics the 
following state-space equations can be derived: 


0 1 0 


>1 ro" 


<j> = 0 0 — <f> + 0 Aii 
J 


0 0 -co 


V CO 


0 1 0 


~o\ ro' 


0 = 0 0 — 0 + 0 All 

J 

V A A V CO 

-J 0 0 —CO L -I L J 


To facilitate the use of an integrator in the feedback structure a fourth state 

can be added to the state vector, which is defined as: ^ ~ ^ or s - 0 and augment this 
state to the state vector, the final roll and pitch models will be: 


0 10 0 


f\ ro 


6 0 0 — 0 (f) 0 (f) 

J + A u — A + BAu 

* 0 0 -<o 0 V “ 

-d 1 0 0 0 Ld L°J Ld 


0 10 0 


0 \ ro 


0 0 — 0 ^ 0 0 

J + A u — A + BAu 

0 0 -G) 0 v ® v 

1 0 0 0 Ld L°J Ld 
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So the state space model has the same state and input matrices A and B 
and so the angles will be the same and only one computation should be done for the 
Gains in LQR problem. The LQR model for the pitch and roll controller is shown in 
Figure 44. The gains if p/l z, K pi i are the outputs of the Roll - Pitch LQR Controller m-file 

shown in the Appendix A. 


ROLL MODEL 


[phi_ref] 
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[u_ref] 
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u ref 
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PITCH MODEL 


'theta 




du/dt 


[u_ref] 


theta dot refl 


u ref 
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-€> 
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Matrix 

lultipl 


upitch 


theta 


theta dot 


Productl 


theta dot 


Roll Model Gain 
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Pitch Model Gain 

[Kpi1.Kpi2.Kpi3.Kpi4] k 



Figure 43. Pitch - Roll LQR controller 
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c. 


Height Dynamics Model 


The motion of the Qball-X4 in the vertical direction (along the Z axis) is 
affected by all the four propellers. Assuming that the same force thus PWM input is given 
for every propeller so the dynamic model of the Qball-X4 in this case can be written as: 

.. 4 f 

Z = —-cos^cos<9 - g where F = Kv (100) 

m 


As expressed in this equation, if the roll and pitch angular rates are 
nonzero the overall thrust vector will not be perpendicular to the ground. Assuming that 
roll and pitch angles are close to zero, using again the Eq (94) for the state variable v and 
a fourth state for the use of an integrator in the feedback structure, the dynamic equations 
can be linearized to the following state space form: 
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( 101 ) 


The LQR model for the Height controller is shown in Figure 44. The gains 
K h i are the output of the Height LQR Controller m-file shown in the Appendix A. 



Height Model Gain 



Figure 44. Height LQR controller 
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d. X- Y Position Model 

The motions of the Qball-X4 along the X and Y axes are caused by the 
total thrust and by changes of the roll/pitch angles. Assuming that the yaw angle is zero, 
the dynamics of motion in X and Y axes can be written as: 

4 F 

x =-(cos (f) sin y/ + cos (j) sin 0 cos y/) 

m ( 102 ) 


y =-(- sin (j) cos i// + cos (f) sin 0 sin y/) 

m 


Assuming that the roll and pitch angle rates are close to zero, the 
following linear state-space equations can be derived for X and Y positions. 


..4 K . 4K 

x = - vsine/w- vv and 

m m 


y 


-v(-sin (/>) 

m 
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~x~\ 


X 


V 


s J 
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1 0 
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III 

-co 0 

0 0 


"7l 


" 0 " 
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+ 
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V 


CO 

s 
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(103) 


(104) 


The LQR model for the X-Y Position controller is shown in Figure 46. 
The gains KJ,K v i are the outputs of the X-Y Position LQR Controller m-file shown in 

the Appendix A. 
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X POSITION MODEL 
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'•Matrix 
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Figure 45. X-Y Position LQR controller 


e. Yaw Model 


As derived in the previous chapter the motion in the yaw axis is caused by 
the difference between the torques exerted by the two clockwise and the two 
counterclockwise rotating props. The motion in the yaw axis can be modeled by: 


if/ = r 


U± 

•4 


K 


yaw 


4 


Au 


Au = u x + u 2 -u 2 -u 4 


(105) 
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The yaw axis dynamics can be rewritten in the state-space form as: 
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“0 
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(106) 


The LQR model for the yaw controller is shown in Figure 47. The gains 
K vaw i are the output of the Yaw LQR Controller m-file shown in the Appendix A. 



Yaw Model Gain 



Figure 46. Yaw LQR controller 
/. Motor Inputs 

The main goal of the controller is to accomplish an algorithm of quadrotor 
control and provides decoupling of control channels in steady state. Thus the control 
inputs from the controller about each axis v.,v g ,v , together with the throttle command 

for each rotor are combined to generate the control inputs u x ,u 2 ,u 2 ,u 4 that are given as 
follows: 






































This control mixing that take place in Figure 48 can be proved to 
be valid within the following derivations, showing that every control achieves the desired 
movement in each associated axis: 


U | — Mj + + U 3 + W 4 

=>U { = 4 u th 


= U th +V g +V v/ +U th +V^ 




■ v e + \+ u th 


U 2 =u x -u,=u lh +v g + v ¥ -(u lh -v e + v ¥ )=> 
=>U 2 = 2v g 

U, = u 2 -u 4 = u rh + v^~ v y/ - (u th -v^-v v )^ 
=> f/ 3 = 2v^ 


t / 4 = u x +m 3 - u 2 -u 4 = u th + v e + V ¥ + u th -v e + v ¥ - (u th +v^-v ¥ + u th - - V ¥ ) => 

=>U<=2v ¥ 



Figure 47. Control Mixing Block 
Figure 48. 
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5. Qbot Inverse Kinematics Procedure 

The model of the Qbot described in the previous chapter was described with the 
forward kinematics. The controller that the robot will use is introducing the inverse 
kinematics problem. The inverse kinematics problem of a mobile robot with initial 
configuration (x,y,9) t=a is the achievement of robot’s target configuration(x, y,&\- if 

(x, y, 1 9) t=0 - (0,0,0), then solving the Eq.89, we will have: 


x(t) = 


y(t) = 


m= 


L i V n,J t )+%,(()] 

2 ( V ri g h,(t)-V lefl (t)) 

2 (v rig *,(0-v^(0) 

L [ V ri g h t (t) + V ,efM 

2 ( v ^ r (0-v fe/r (0) 


sin [^(v righl (t)-v, efl (t))\ 
cos [j(v right (t)-v left ( t ))\ + 


L \- V ri g hXt) + V,efi(t)'\ 

Wright (!) - V le/t(0) 


(107) 


For a known time t and a target position (x, y), the last Eq. 107 will be solved for 
v right> v left • f- c l s - 83 and 84 provide a simpler strategy to drive a robot to a target position 

(x, y, 9) t where the robot can be rotated in place until it will be aimed toward (x, v), then 
driven forward until it will be at (x,y), and finally rotated in place until the required 
target orientation i9is met. From the first two equations of Eq. 80 we obtain 

■*' w =2>/F77 


v +1 

y right * left 


Also, form the same two equations we obtain & = arc tan 


T 

v xJ 


(108) 

(109) 


Differentiating Eq. 110 yields & = ^(110) 

x +y 

V . - V 

From the other hand, the third equation of Eq.80 reads 9 = —— (111) 

Hence, K lgh -V lef: = (112) 

x +y 

Eq. 108 and 112 resolved for Vright and Vleft yield the controls required to follow 
a predetermined trajectory y(x). 
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C. DIRECT METHOD BASED CONTROLLERS 

1. Introduction 

The classical indirect methods cannot handle complicated problems in real time. 
There is a need for different techniques that simplify the problem or use numerical 
algorithms to provide near-optimal rather than optimal solutions in real time. The 
proposed controller introduces one of the direct methods already worked in real time. It is 
quite easy to program and provides effective optimization with feasible solutions. Taking 
Prof. Taranenko’s ideas in early 60s as motivation, the computer’s rapid development 
helped several engineers develop algorithms for real-time on-board calculation of quasi- 
optimal trajectories ([41]-[43]) for combat vehicles and missiles. There were successful 
tests within the Pilot’s Associate program onboard 5th-generation aircraft [44], This 
direct method can be used for unmanned air vehicles (UAVs) to generate approach 
trajectories. 

The direct method’s simplicity gives one the opportunity to develop the theory, 
and eventually test a real-time trajectory optimization scheme for the quadrotor. In the 
following sections the general architecture of the proposed controller will be developed, 
consisting of the trajectory generator and the trajectory follower. Using the parameters 
and dynamics of the already working controller we will introduce the trajectory generator 
which will generate optimal or quasi optimal feasible trajectories so that follow the 
waypoint pattern of the ground robot. This real time capability provides regeneration of 
each trajectory during the mission through feedback from the sensors. It allows for 
changing the objectives for any disturbances. To accomplish this task, a reference 
trajectory and the controls have to be found, so that even with LQR controller, the 
following of the path will be also accomplished. Also, an interpolator must be used to 
provide samples of the reference trajectory at the desired (high frequency) rate. The 
proposed scheme is shown in Figure 49. This control scheme differs from the previous 
one in the fact that in order to achieve a solution, the trajectory generator and the 
controller both need to follow the reference trajectory. 
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Figure 49. General Architecture of Joint Quadrotor - Robot Controller 
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2. Formulation of the LQR Problem 

The objective is to find a controller that provides the best possible performance 
with respect to the given criterion. The requirements of the controller are: 

• Optimality 

• Stability of the closed loop system 

• Desirable gain and phase margin 

• Robustness with respect to unmodelled dynamics of the plant and 
environmental uncertainties 

For a linear state-space model of the system dynamics: 

X{t) = AX(t) + BU (t) (113) 

With an assumption of availability of measurements of full state the matrix gain 
K c of the optimal control input must be determined: 

U(t) = -K c X(t) (114) 

So as to minimize the performance criterion or the cost function 

oo 

J = \{X T QX + U T RU)dt (115) 

0 


where Q and R, are positive definite Hermitean or real symmetric weight matrices. In fact 
Q must be positive semi definite. 

Bryson’s Rule defines an initial choice of matrices Q and R 


Qu = 



_l_ 

max acceptable _ value _of _ Xf ’ 

_1_ 

max acceptable _ value _ of _ U ^ 


ie{l,2...,n} 
j e {l,2...,«} 


(116) 


which corresponds to the following criteria 

^ n n 

=j(£ qx w+ i R^rndt 

o 1 1 


(117) 
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For a time dependent reference trajectory X ref {t ), the LQR control can be applied 

as a trajectory follower to minimize small errors between the measured state x and the 
reference state X ref , and the control algorithm will become: 

U(t) = u ref (0 - K c (X(t) - X ref (0) (118) 

We have to compute the gain matrix K c using a linearized quadrotor’s model and 
then apply this control the non-linear model. 


3. Stability Analysis 

Since the linearization of the quadrotor plant is already taken place at hover 
condition, the control gains K c were designed with the weighting matrices as follows: 

2 = 10 5 / 12x12 and R = dzag(lCT 5 ,10 8 ,10 8 ,10 8 ) , 

if it is sure that the actuator constraints are maintained as stated in [62], However, 
following the generated trajectory the hover condition will be sometimes violated. That’s 
why a specific envelope where the operation will be stable has to be detennined. As 
already derived in [63] a linearized stability set representing a circumference of a circle 
with a radius of 48 degrees can be practical enough: 

s(t) = {O,<f>:0 2 +<j> 2 <r 2 ,r = 48°} 

So if an extra constraint added to the optimization, that maintains angles (f) and 0 
within this stability set within the trajectory generator the linearized time-invariant 
stability will be assured. 


Now that we set up the problem, we can see the whole flow procedure within a 
diagram, in order to visualize better the technique to be used. The direct method 
optimization flow procedure is presented in the following chart, where each component 
shown will be described in the following sections of the chapter: 
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Figure 50. Direct Method Optimization Flow Procedure 
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4. Reference Trajectory 

To approximate the Cartesian coordinates of a vehicle, its speed and its 
acceleration (six states), we can use functions that will define the variety of accessible 
trajectories and their choice will depend on the particular problem. In general, the more 
terms those functions have, the more accurate (closer to the really optimal) solution can 
be found. Once the Cartesian coordinates, speed and acceleration are defined using 
reference functions, the remaining states and controls are detennined using inverse 
dynamics of original non-linear equations driving the system’s dynamics. By using the 
direct method as Taranenko suggested in [63], a big advantage over the indirect method 
sis obtained since we eliminate the issue of solving the Cauchy problem for detennining 
the trajectory with the given initial states and control time-histories, but instead, 
introducing the desired trajectory from the beginning, time-histories for all the controls 
are retrieved through the use of the inverse dynamics. 

One modification of Taranenko’s method is to employ elementary polynomials 
[44], [45], [62]-[64] as the reference functions. There are other alternatives, like 
Chebyshev polynomials [65], [66] or Laguerre polynomials [67] and others. In order to 
compute the coefficients, let us consider as reference function, algebraic polynomials of 
degree “n” for x, y, z coordinates and use as argument the virtual arc “x”, given as 
follows (the exact same procedure will take place for the other coordinates, y and z): 


,(*■) = 


k=0 


'(T) = Z< 


(max(l,£-2))!r A 

~k\ 

(max(l,£-2))!r 


k -1 


*=i (k-\)\ 

*/(*■)= 2 X t *~ 2 


k=2 


h’(0 = J(£-2 )a ik r 


k -3 


k =3 


(119) 


The degree “n” of these polynomials is chosen from the boundary conditions, 
where they have to be specified accordingly so that all the coefficients a ik will be 
determined algebraically. The higher the maximum degree of the time derivative of a 
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vehicle coordinate at initial and end points, the higher the degree of the polynomial. The 
minimum degree of the polynomial to be chosen will be given by the equation: 

n = d 0 +d f + 1 (120) 

where d 0 ,d f are the maximum orders of the time derivative of the coordinates at the 

initial and end points, respectively. So that at the boundary values for the quadrotor 
coordinates, the first and second time derivatives at both ends of the trajectory are 
satisfied, fifth-order polynomials should be chosen since d 0 = d f = 2 . Applying Eq. (85) 

we define eighteen unknown coefficients. 

Generally, the final part of the trajectories needs to be smoother, since the control 
has to be more accurate while at landing or at rendezvous point. That’s why 
d f = 3,jc'" r = 0(/ = 1,2,3) is usually proposed. 

In case of d {) = 2, d f = 3, thus n = 6, where an additional optimization parameter 
is applied, then 24 coefficients a ik are obtained. Subsequently, if we add two parameters, 
the coefficients will be 36. 

Expressing the six coefficients a xk ,k = 0,1,...5 (the same manner for a Yk and a /k ) 
as a linear matrix equation, we will have [64]: 


10 0 0 

0 10 0 

0 0 1 0 

, 1 2 1 3 

1 Zf —Zf —Zf 

f 2 f 6 f 

0 1 z f — zl 

f 2 f 

0 0 1 z f 
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0 
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-1 
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1_ 
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a x\ 
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1 4 

1 5 

a x2 



— 1 f 
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— T f 
20 f 
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1 3 

-z f 
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— Z f 

4 f 

3 

a X4 

_ a X5 _ 


1_ 


( 121 ) 
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The reference functions provide us with the flexibility to increase the order of 
approximation and derivatives at both ends using them as additional varied parameters. In 
the previous case, the only varied parameter is r / (since all coefficients are determined 

from the boundary conditions), shown in Figure 54. The diagrams are produced very 
easily if you calculate the equation 121 in Matlab. 



Figure 51. Variation of the parameter of the reference functions, t , = var 


If 7th-order polynomials are used, the initial and final state, first and second 
derivatives were used as constraints attempted to be satisfied and the third derivatives at 
both ends of the trajectory became free variables along with the virtual arc length [64]: 

x'(t) = P x . t (r) = a, 3 + a t4 r + a i5 r 2 + a i6 r 3 + a„ r 4 


1 . 2,1 .3 1 


1 


x](j) = PAr) = a n +a G T + -a i 4 T z + -a ;5 r J + ^a i 6 r q +^a n r 5 (122) 

x'(r) = P r . (r) = a,, + a n r+ — a n r 2 + — a iA z 3 + — a.,r 4 +— a lf r 5 + — a n r 6 
! x,v '* 12 2 6 12 20 30 

x.(r) = P„(t) = a m +a n r + — a n T 2 + — o,.,r 3 + — a iA r 4 + — a iS r 5 +-^—a i j b +-^—a n T 1 

2 6 24 60 120 210 
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The coefficients will be computed by solving the following system of linear 
algebraic equations [64]: 
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(123) 


If these equations are resolved for the coefficients a ik in Matlab will yield [64]: 


a i o x m a n x a 


-4x, y +16x, 0 60x, r -120x, 0 -360x, r -480x, 0 840x, r -840.x, 0 

% =---+-“3-+-S- 


V 






'/ 


30x y + 60x, n -420x r + 600.x, n 2340x, r - 2700x, n -5040.x, r + 5040.x, n 
a i5 = - tf — -- +-^-- +--- +- *— -—(124) 


'/ 


'/ 


V 


-60x, r -80x ;0 780.x,,-900.x, 0 -4080x, r - 4320x, 0 8400x, y - 8400x,. () 

% =-3-+-3-+-3-+-—i- 


'/ 


'/ 


'/ 


'/ 


35x r + 35x,. 0 -420x, r + 420.x,. 0 2100x, y + 2100x,. 0 -4200.x, r + 4200x,. 0 

o =--- 1 - - - 1 --- 1 --- 

n _4 ^ _5 ^ _6 ^ 7 


'/ 




'/ 


X f 


and the final arc x, becomes the first optimization and varied parameter. 
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5. 


Time and Space Decoupling 


Since the speed is related to the Cartesian coordinates by the equation [71]: 

V(t) = Jx(t) 2 + Y(tf+Z(t) 2 


we also specify a speed profde along the trajectory by using time t, as an 
argument, ending to define the trajectory itself, too. 

In order to decouple the trajectory from the speed profile we can utilize the 
abstract argument r which connects to time through the variable speed factor 

(125) 

at 

By this way, we manage to vary the speed profile along the same trajectory by 
changing the speed factor/l(r) [71]: 


V(t) = A(t)^X\t) 2 +Y\t) 2 +Z\t) 2 


(126) 


For the quadrotor’s case though, it is more useful to approximate the speed factor 
with the same procedure as for the reference trajectory before in order to achieve 
parameterizing the speed factor so as afterwards compute the speed profile. 

So, if we assume that: 


«r)=i*; 0na0 '*- 2Wr ‘ 

k=o k ! 

A'W = f a i (max(u ~ 2))!r> "' 


-i»=X a * v ' ! 

k=2 

X\r) = f j (k-2) a y- 3 

k= 3 


(127) 
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And for n=5: 


5 

A(z) = P(z) = ^a 2 z k = + a 1 V + a^z 2 + a 2 z 2 + a 2 z 4 + a 2 z 5 

k =0 


where coefficients are the solutions of: 


1 0 0 0 0 
0 10 0 0 
0 0 10 0 


1 z f — z f 

f 2 f 

0 1 z f 
0 0 1 
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-z, 


0 

0 

0 

1 

— z 
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r *1 

a o 


>o" 

a? 


K 

a 2 


K 

a 3 


v f 

a A 


K 

k 


Aj- 
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(129) 


Then, finally, the speed profile will be computed as: 

V(Z) = P A (z)j(P:(z)) 2 +(p;,{z )) 2 +{R(z)) 2 


(130) 
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6. Qball-X4 Inverse Dynamics 


a. Differential Flatness 

Suppose the dynamics of a system are described by a set of ordinary non¬ 
linear differential equations: 

x = f(x,u ) ( 131 ) 

where x(t ) e X c R" is the state vector and u{t) e U c: M" is the control vector, and f is 
some vector function. 

By definition from [68], “the differential flatness is the expression of the 
state and control vectors in terms of some output vector, y.” (Without loss of generality 
we assume y to be a subset of x, i.e. y = Cx , where C is a k-by-n matrix, k < n). Also 
from [69], “For a system to be differentially flat and therefore possess a flat output it 
requires a set of variables y{t) — Cx e Y a R A such that: 

• The components of y are not differentially related over A ; 

• Every state and control may be expressed as a function of the 
output vector y and a finite number of their time derivatives, i.e. 

x = hfy,y,y,y,...) and u = h 2 (y,y,y, 

which is essentially the inversion of the original system jc = /(x, u) with respect to the 
output vector y = Cx .” 


b. Quadrotor’s Inverse Dynamics 

Attempting to address the differential flatness property in quadrotor’s 
dynamics, the output vector will consist by four components, since we have four controls. 
These components will be the translational positions x, y, z, and the yaw angle y/, as it 
can be dynamically decoupled from the other states [70] (in case the control input U is 
used to set the yaw angle to zero). 


So, the output vector Y can be defined as: 


7 = [; 


y 


H r = 


Ax3 ^3x 5 0 0 3l . 3 


0 0 


1x5 


1 0 


1x3 


x = cx 


(132) 
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Now, the control inputs can be expressed as a function of the states and their 
derivatives as follows: 


U x = jx 2 +y 2 +(g + z) 2 

U 2 =0 

U 3 =9 

U 4 =ys 


(133) 


We have to take the second derivative of the states </>,6 , the rotational part of the 
state vector, in order to express all the control inputs toward output vector. So, in order to 
express those states in terms of the output vector, we follow the procedure below: 

If we take individually the three equations of Eq. 90 and by rearranging and 
multiplying the third one with “tanO” we have: 

x = U l sin (j) sin y/ + U 1 cos (j) sin 9 cos y/ 
y = -U l sin <f) cos y/ + U 1 cos (j) sin 9 sin y/ 

(z + g ) tan 9 = U l cos (f> sin 9 

If we substitute the third equation to the other two, we get: 


x = U x sin (f> sin y/ + (g + z) tan 0 cos y/ 
y = -C/j sin <j) cos y/ + (g + z) tan 9 cos y/ J 


xcos y/ 


xsin(// 


x cos y/ = U 1 sin <j) sin y/ cos y/ + (g + z) tan 9 cos 9 y/ [ ( + ) 
v sin y/ = -C/j sin (f> sin y/ cos y/ + (g + z) tan 9 sin 2 y/ I 


x cos y/ + y sin y/ = (g + z) tan 9 cos 9 y/ + (g + z) tan 9 sin 9 y/ 

x cos y/ + y sin yr = (g + z) tan 9 
x cos y/ + y sin y/ 


tan 9 = 


g + z 


9 = arctan 


xcos^ + ysin^ 
g + z 


(134) 
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With the same procedure if we multiply with the opposite way and subtract the 
two equations we get: 

x = U x sin (ft sin y/ + (g + z) tan 9 cos yr 
y = —C/j sin (ft cos yr + {g + z) tan 0 cos yr 

xsinyr = U x sin^sin 2 y/ + (g + z)tan^sin^cos^ | (-) 

=^> 2 f=> 

y cos y/ = -U x sin (ft cos' y/ + (g + z) tan 6 sin y/ cos y/\ 


I xsin^ 


' xcos^/ 


=^> x sin yr - y cos y/ = U X sin ^sin 2 y/ + U ] sin (ft cos 2 y/ 

=^> xsin^-jicos^ = U X sin (ft (y^yyjyr^rS(xcy^ 

. , xshw-vcosu/' 

=> sin (ft =--—--— 


=> (ft - arcsin 


x sin y/ - y cos y/ 

sjx 2 + y 2 Hg+z ) 2 


(135) 


Having the state vector components expressed in terms of the output vector, we 
proceed to do so for their derivatives also are: 


and 


e = 


(3c cos y/ + y sin yr)(g + z) - (x cos y/ + y sin y/)z 


(g + z)" +(3ccos^ r + ysin^')" 


• (3c cos yr + y sin yr)(g + z) - (g + z) (tan Oz) 

0 =--—^j- 

(g + z)‘ +(3ccos^+vsin^)" 


6 - 


(vcos(// + ysiny/- tan ffz) (g + z ) 
(g + z)' +(xcos^ + ysin^)" 


(136) 
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<j) = 

i> = 


(x sin y/-y cos y/)U l - ( x sin y/ - y cos y/)U l 


U iy /U 2 -(xsiny/ -ycosy/y 
(x sin if/ -y cos y/)U l - U { sin <j>U l 
U^U 2 -(Jcsin^- ycosif/)" 

(x sin y/ - y cos y/) - U x sin </) 


<t> = 


U 2 -(xsin^-vcos^)~ 


(137) 


The second order derivatives: 


S = (g + z) 


(x cos y/ + "y sin y/ - tan 9'z) - 26 [z + (x cos y/ + y sin y/"j tan 6 1 ] ^ 


(g + z) 2 +(xcos^ + ysmy/Y 


(x smy/-'y cosy/)-U x sin^ (j) U l -sin^(x sin^ - y cosy/') U l 

<!>= - 1 - ... . --- 72 - ( 138 ) 


U 2 - (x sin y/ - ycosy/y 


U 2 -(xsiny/--vcosy/') 


Instead of using the exact values, we could also employ central difference 
approximations 



6 


-20,-6U 

At 2 


4-24-4,, 

% At 2 


(139) 


(forward and backward approximations would be used then for the first and last 

points). 

Finally, we computed the states <j>,6 in terms of the output vector components, 
since U l is already expressed in terms of x, y, z. When the vehicle is in free fall, 
singularities may occur, since g = —z. One way to avoid it is by constraining the input 
such that U ] > 1 and the pitch and roll such that 6 < 90° and (j) < 90° as stated in [48]. 

The differential property of the system provides us with the opportunity to transfer the 

optimization from the control space to the Output space. 
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7. 


Discretization 


As in any numerical method we will compute parameters of the systems in a finite 
number of points N placed equidistantly along the virtual arc z f (varied parameter), thus 

by dividing the virtual arc r f into N -1 equal pieces as shown in Figure so as [71]: 


Ar = z AN -\) 


(140) 


r * 1\ r * s = Vt | 


Ar 


V, 


V,=V n 


At 


A U 


At 




r = [0:r / ] Ar = 


Ar Ar 


N-l 


At 


At 


x v = x / 

A 


Ar * 


V, 


N 


v 

V v-i 

v N-l 


x(r) 


At 


A fv-i 


N-1 


Figure 52. Excluding Time and Converting Back to Time. After [71] 


We then have N equidistant nodes / = 1 N. All states and controls at the first 
point j = 1 (corresponding toz-j = r 0 = 0) are defined. For each of the subsequent N-l 

nodes j = 2,.., N, the corresponding instants of time, however, need to be computed as 
follows [71]: 


r = (J -l)Ar => Ar = 


7-1 


A^-i 


2Ar 


Aj-i + Aj 


tj - t hX + Atj_ x 


(141) 


(142) 

(143) 


So, the mapping between the r and t domains are defined by the speed factor 
profile P x . We can now proceed with computing the states and controls in all nodes. We 
compute the current value of three Cartesian coordinates and speed using the 
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corresponding polynomials: X } = P x (r j ), Y. = P Y ( v .) and Z i _P z (T j ). Then, using the Eq. 
141 we compute the time passed since the last sample: 


At 


J~ l nr rr \ 




(144) 


The current time then from Eq.143 equals to t j =t j _ l +At j _ l (t l =t 0 = 0), and 
therefore the current value of the speed factor 

At 


A,= 

1 At 


(145) 


i -1 


Next, we convert r derivatives to time derivatives using chain rule relations: 


x = — = —— = x A, and x = ^ = ( X 'A + x A)A , so that we finally have: 

dt dz dt dt 


Xj = AjiAjXj + Ajx]) 

Xj = Aj[(Xr + XjA])xj + Aj(3AjXj + Ajx])\ 
yj= A jy'j 

y j =Aj(Xjy' ] +A j yj) 

y j = 'tjK'tj 2 +X ] X J )y j +A j (3X j y j +A j y J )\ 


AjiXjZj+A/j) 

A^Xr+X/^+A^X^+A/])} 

Vi,j = Aj (Xji/z'j + AjYj ) 

To convert the initial conditions of the states from time derivatives to virtual arc 
derivatives, we have to use the inverse of the above relations, so as to compute the 
controls and the remaining states. 
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8. Trajectory Optimization 

We have already chosen the reference functions for the X, Y, Z, 'F and Zand 
computed their coefficients, introduced the inverse dynamics to the system and set the 
boundary conditions. The next step is to proceed with the trajectory optimization through 
a specific optimization routine. 

a. Problem Formulation in the Control Space 

Normally, in order to determine the optimal trajectory, the optimization 
procedure take place within the control space regarding the applied constraints like state 
constraints, actuator (control) constraints and obstacle avoidance constraints within the 
output space and the state space. The problem can be set up as [48]: 

min ® for t e To, t , 1 such that 

U(t)eUc.% A L 1 J 

Y-Cg(X,U) = 0 

Y 0 -Cg(X 0 ,U 0 ) = 0 

Y f —Cg(X f ,U f ) = 0 

C(X,U) < 0 (145) 

where O(Y, U ) is the cost function, the initial and final constraints on the states are set 
according to Eq.132 and Eq.88 at / = Oand t = t f , respectfully, the dynamic inequality 

constraints on the trajectory (for obstacle avoidance) and on the states and inputs (to 
avoid singularities and to provide constraints on the control signals) are expressed 
through the set of functions C(X,U). 

b. Cost Function 

The cost function, O, is a quantitative measure of the optimality of the 
trajectory and consists by the sum of two components, the running costs and the terminal 
cost. If the running costs (battery consumption) are proportional to the average velocity, 
the objective function can be defined as [48]: 
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( 146 ) 


® = (1 - w )-1 ^p x x 2 +P l y 2 +P l z 2 dt + w(t f - Tf 
t o 

where w, f] P 2 P, are the weighting factors (not necessarily equal to each other), and T is 
the predetennined time of arrival. The minimum-time case takes place when w=l and 
T=0 and the minimum-fuel case when w=0 and P l -P 2 -P } . So, it is clear that the 

mission scenario will determine what the cost function is and how many weighting 
factors have to be adjusted. 

Having proved through differential flatness that the state vector x and the 
control vector u can be both expressed via the derivatives of the output vector y 

X = h l (Y),U = h 2 (Y ) (147) 

We can reformulate the optimization problem in the output space. 


c. Problem Formulation in the Output Space 

If optimization takes place within the output space (differential flatness 
properties) as opposed to the control space, it would be very useful because the 
constraints occurring for example from obstacle avoidance happens in the output space, 
hence the computation time for constraint handling is reduced drastically. As opposed to 
(145) the problem can now be refonnulated as follows: 

min ® for t e |~0, t f 1 such that: 

[/(OeUc'Ji 4 L 1 -I 


*W’(r(0)) = o 

Y f ~g*(tf) = 0 
C*(T) < 0 


(148) 


where = C S( X ’ U ) = Cg(h 1 (Y),h 2 (Y)) C\Y) = C(X,U ) = C{h x {Y),h 2 (Y)) come 

from Eq. 147. With proper parameterization this problem can be solved in MATLAB 
using the optimization toolbox function fmincon. 
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d. Parameterization 

In order to reduce the dimension of the problem to a finite amount, it is 
suggested that the three translational outputs (x, y and z) be parameterized (the fourth 
output, the yaw angles, is assumed to be zero). 

Thus the equations derived so far will become: 


6 = arctan 


x 

g + z 


(149) 


(/) - arcsin 


y 


■yjx 2 +y 2 + {g + zf 


(150) 


The derivatives also are: 


^ = x( g+ z)-m) or 

(g + z)~ +x 2 


(x-tan 9z)(g + z) 

(g + z) 2 +^ 2 


<t> 


^_-y^x 2 +y 2 +(g + z) 2 +y(xx+yy + z(g + z)) 


x + y'+(g + z ) Jx + (g + z ) 


or 


(151) 


0 = 


-yU l +yU l _-y-U l sin^ 


(152) 


The second order derivatives: 


« = (g + 2) 


(x - tan 9"z) - 29^'z + (x + y) tan 6*] ^ 


(g + z ) 2 +* 2 


(153) 


* = 


[(-y )-t7 1 sin^] ^[t/j -sin^(-y)]t/j 




u 2 -f 
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V. SIMULATION 


A. QUADROTOR MAIN INTERFACE 

The Quarc / Simulink top level realization of Quadrotor model and controllers 
developed are shown in Figure 54. This the main screen you see when you open the 
quadrotor model in Simulink. It was separated into different components for friendly use 
by the operator. 

Qball-X4 Waypoint controller 

Issue waypoints to the Qball for navigation control. 

Switch between joystick and closed-loop control using the switches inside 

the Mode Control subsystem. j 

' j Bad Link 

In closed-loop flight, control the position of the Qball-X4 by setting height 

and heading in the Position Commands subsystem. 

View IMU data and motor output signals in the HiQ subsystem. 

Data is logged to a host MAT-file in HiQ\SAVE DATA (black box) 



I 1 , Pitch Controller Roll Controller Yaw Controller Control signal mixing 

HiQ Joystick from host 

Figure 53. Simuli nk Representation of Qball-X4 Controller 

The system consists of the following subsystems blocks: 

1. Positions Commands Subsystem 

The Position Commands subsystem contains the waypoint state machine 
described in the previous section, where its outputs are the throttle command, the x, y, z 
position and the heading commands as shown below in the simulink diagram. The Y axis 
is called Z inside the optitrack software. So, where Z is meant Y and where height is 
meant Z. 
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Gain Heading 


Heading command (active when heading_mode set to 1) 


heading command Goto€ 


O‘pi/180 


< 


heading_cmd 


Figure 54. Positions Commands Subsystem 


As far as the heading command is concerned, a stable heading set up to 8 degrees 
was used for the experiments. This eliminates the small disturbance of the heading when 
the quadrotor takes off. I assume this phenomenon exists because of the metallic objects 
inside the lab that cause some interference in the magnetometer measurements. 

2. Mode Control Subsystem 

The Mode Control Subsystem allows the operator to select height, position and 
heading modes for autonomous or 4-channel joystick control as shown in Figure 56. 
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CLOSED-LOOP ON 


Set to 1 for automated height control using sonar. 
Set to 0 for joystick control over throttle. 



height_mode 

Goto3 



Set to 0 for joystick roll and pitch control. 

Set to 1 for autonomous position oontrol using OptiTrack. 


position_mode 


Goto8 



Set to 1 for autonomous heading control using magnetometer 
Set to 0 for joystick yaw oontrol. 

NOTE: first make sure the magnetometer is calibrated and the offset is set in the field 
HiQ\Calculate Roll Pitch Heading Height\Calculate Heading .camera frame heading offset 

heading_mode 

Goto2 


Figure 55. Mode Control Subsystem 


3. Calculate Roll Pitch Heading Height Subsystem 

This subsystem computes the vehicle’s orientation through the calculation of roll, 
pitch, magnetic heading and the x, y magnetometer components of the quadrotor. 
Complementary filter was used to correct roll and pitch components. 


Calculate Magnetometer Calculate Magnetometer 

X Component Z Component Calculate Heading 




Figure 56. Calculate Roll Pitch Heading Height Subsystem 
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4. HiQ Subsystem 

The HiQ subsystem consists of the Hardware- In-the-Loop (HIL) blocks used to 
configure the HiQ acquisition card and read or edit the values. It contains the Motor / 
PWM outputs that power the whole system and initialize the sensors to get the 
measurements through the various selectors. The Gain simuli nk block enables or disables 
the motors by multiplying the motor input signals by the value of 1 or 0, respectively. 
Inside this subsystem you can also check the battery voltage. 



Gbal (hiq_aero-Q) 


Motor outputs: back, front, left, right 




HIL 

Watchdog 


HIL Watchdog 
(Gbal) 



watchdog 



— H u Y l - 

Select accelerometers 

accelerometer x, y. z 



Select magnetometers - 

magnetometer x. y. z 



- H u Y l — 

Select battery 




Ini battery voltage 


battery voltage 


» |u Y | — 

Select sonar 



sonar 


Figure 57. HiQ Subsystem 


112 








































































5. 


Joystick from Host Subsystem 


This subsystem is an alternative power system to the motors through the 4- 
channel joystick. It receives streaming data from the host model. If the IP address is set 
up properly for the ground station in the Stream Client URI, then the Qball is able to 
connect to the host model. It gives the same commands as in the Position Commands 
subsystem and receives the data packet from the optitrack system. For safety reasons, if 
the communications from the host is interrupted for 1 consecutive second, the QBall is 
ordered to land. 



Stream Client2 


"tcpj>://182.168.1.219:18006' 

Figure 58. Joystick from Host Subsystem 
6. Save Data Subsystem 

The “Save Data” subsystem saves all the data collected from every flight of the 
quadrotor into a MAT file through the Quarc block “To Host file” for further processing 
or plotting. Figure 60 shows how all the different signal outputs collected and saved. 
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Figure 59. Save Data Subsystem 


B. QBall X4 WAYPOINT NAVIGATION 
1. Waypoints Input 

The waypoints foe the vehicle’s navigation will be introduced through the script 
“Initialize_Qball_Waypoints.nl showing in Appendix A, where the user defines the 
position of the waypoints in the operation X-Y area (again Y axis is represented in the 
model as Z because of the optitrack system notation). The platform where the waypoints 
are set is shown below: 



Figure 60. Waypoints Input Diagram 
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2. Waypoint State Machine 

The waypoints will be followed through the waypoint state machine shown in 
Figure 62. 



Waypoint stale machine 


Figure 61. Waypoint State Machine 

The height will be held constant at 0.6 m (hgt coordinate) through the sonar 
controller. Tx and Tz are the coordinates taken from the previous section and represent 
the coordinates of x- and y-axis, respectively (This representation came up from the 
optitrack system set up). The function controls the machine is shown in Appendix A. 
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3. Waypoint Tracking 

The trajectory will be followed through the implemented LQR controller 
described in the previous chapter and for specific weight factor matrices that will 
improve the performance. The LQR gains are shown below: 

Roll - Pitch Controller Weight Factors: Q= [100 0 22000 10] - R=3000 

Output LQR Gains: k(l) = -20.047 + 0.000 i 
k(2) = -5.618 +6.128 i 
k(3) = -5.618 -6.128 i 
k(4) = -0.316 +0.000 i 

X-Y Controller Weight factors: Q= [50 2 100 0.1] - R=50 

Output LQR Gains: k( 1) = 12.080 + 0.000 i 
k(2) = -1.762 + 1.604 i 
k(3) = 1.762 - 1.604 i 
k(4) = -0.045 + 0.000 i 

Height Controller Weight factors: Q = diag([l 0 50]) - R = 5000000 

Output LQR Gains: k(l) = -0.517 + 0.890 i 
k(2) = -0.517 - 0.890i 
k(3) = -1.024 +0.000 i 

Yaw Controller Weight factors: Qy = diag([l 0.1]) - Ry = 1000 

Output LQR Gains: k(l) = -3.762 + 1.287 i 
k(2) = -3.762 -1.287 i 

The representation of how accurate is the waypoint tracking is shown in figure 63 
(two trajectories). The quadrotor takes off, stabilizes in the assigned altitude and then 
follows the waypoints 1 to 6 added from the operator (shown in Figure 61), where the 
starting point is the zero point (0, 0) of the localization system. When each waypoint is 


116 



found, the quadrotor has to stay 5 sec. A little overshoot takes place because of the 
acceleration, it has already gained. The smaller the distance the smaller the overshoot. 
The second trajectory takes place with low battery. Through the experiments, it was 
shown that as the battery goes off; the response of the quadrotor is degraded. 




Figure 62. QBall Actual Trajectories ( With Full battery and low battery ) 
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4. 


Plots 


Several plots can be obtained using the Save data interface. Some of them can be 
really useful for understanding the response of the vehicle and the controller; some others 
just for testing purposes in order to check if we can obtain data from the quadrotor 
sensors. The PWM input of every motor toward time is shown Figure 64. The initial 
value is the 5% of the 20 ms, thus 1 ms that is the minimum throttle and it goes up to 9% 
of the 20 ms, thus 1.8 ms (maximum throttle=2 ms). 
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Figure 63. PWM Input for each motor 


The following figures depict the data from each sensor and can be used for testing 
purposes. Figure 65 shows the gyroscope measurements in rad/s, where small variations 
around zero take place. Figure 66 shows the accelerometer measurements in m/s. The Z 
acceleration oscillates around -10, the value of the gravitational acceleration. Figure 67 
shows the magnetometer measurements in Gauss, with the Z value to be the largest. 
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Figure 64. Gyroscopes measurements 
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Figure 65. Accelerometers measurements 
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Figure 66. Magnetometer measurements 
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The observable and magnetic heading are shown in Figure 68. The magnetic 
heading is the straight calculation of the atan2 (mag_y / mag_x), where mag_x, mag_y 
are the measurements of the magnetometer. The observable heading is the corrected one 
using feedback from the magnetic heading. 
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Figure 67. Observable and magnetic heading 


The following plots present the battery voltage and the chosen modes for 
autonomous navigation. If the mode is 0 the navigation is taking place through the 4 
channel joystick, else if the mode is 1 then the navigation is autonomous. The battery 
voltage drops from flight to flight, if the voltage goes under 10.6V then the battery 
becomes useless. 
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Figure 68. Battery voltage and height-heading-position mode (auto=l) 
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The following plots in figures show the comparison of the measured and the 
commanded X, Y, Z data. The small differences shown is having to do with the 
disturbances occurred during the flight as well as with the linear controller it was used. 
The difference in height between measured and command Z is because the optitrack 
system captures the reflector placed in the top of the quadrotor, thus 0.6 m from the 
ground. 


- X command 

- X measured 



Figure 69. X- Y position comparisons 
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Figure 70. Height position comparison 


The comparison of the measured and commanded roll and pitch data is shown 
below. The difference actually in both roll and pitch response is quite small. The 
measured roll and pitch values are already filtered, as it is noticed form the graphs. The 
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only distinct difference is happening in the first seconds when the takeoff takes place. It 
is a phenomenon under investigation since the quadrotor tends to roll a little bit as it takes 
off and climbing to the assigned height. The roll controller gains should be tested more in 
order to eliminate this effect. 
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Figure 71. Roll comparison 



Figure 72. Pitch comparison 
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B. QBOT WAYPOINT NAVIGATION 


1. Waypoints Input 

The waypoints will be introduced through the script “Initialize Qbot Waypoints.m 
showing below where the user defines the position of the waypoints in the operation area 
(x-y area). 

As we see in the m- file, there is a calibration file loaded, coming from a magnetic 
calibration process that took place before the navigation process began, in order to set up 
the compass to the environment of the laboratory taking into account the declination of 
Monterey that is different from Toronto (Quanser company location) where the initial 
calibration was done. The simulink model of the compass and the model that initiates the 
calibration are shown in Figures 74 and 75, respectively. 



Compass 


Figure 73. Compass Model 
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Figure 74. Qbotmagnetometercalib model 
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The Calibrated magnetometer x (red), y(green) measurements are shown in Figure 
76. The difference in time occurs because of the offset imposed for better representation. 
The calibration is succeeded when the values come closer to the absolute 1, -1. So the 
performed one is better for the y measurements. 
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Figure 75. Calibrated Magnetometer X, Y Measurements 

The platform where the waypoints are set are shown in Figure 77. Six waypoints 
are chosen. The Qbot is placed in the zero point of the localization system. 


Calibrated Magnetometer X, Y Measurements 




Figure 76. Waypoints Input 
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Waypoint Tracking 


The waypoint tracking takes place through the motion planner which uses the 
inverse kinematics. It takes as inputs initial and target position of the robot, the time step 
and after implementing a feedback structure, computes the right and left velocity and the 
distance to the target position. 



Figure 77. Motion Planner Simulink Representation 
3. Trajectory representation 

The trajectory for the waypoint tracking is shown in Figure 79. The Qbot starts 
from zero point and follows the assigned waypoints. The curves shown in each waypoint 
is the rotation of the Qbot to the required heading and they are being shown because the 
diagram tracks the velocity, too. The Qbot computes the right and left wheel velocities 
required for following the trajectory from one waypoint to another through the inverse 
kinematics, and with the forward kinematics moves toward each waypoint. It also takes 
input from the compass and provides feedback. This is the alter to the initial trajectory 
between waypoints 3 and 4. 
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4. Plots 


The following plots depict the actual x, y position components and the orientation 
theta of the Qbot as well as the optitrack measurements of these components for the 
waypoint pattern described earlier. The difference between the encoded data and the 
optitrack data is having to do with the fact that the optitrack system captures the motion 
of the reflector placed on the top of the camera and not the robot itself. The “topt’ 
diagram is so “messy” because the robot adjusts its position and orientation several times 
till it finds itself within the limits of each waypoint. 
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Figure 79. Qbot x, y, theta plots 
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In order to find the final orientation theta that the robot will apply to go to each 
waypoint, these components have to be measured. “Dist” is the wheel’s distance travelled 
in a sample time, “Ang“ is the angle turned in a sample of time, “heading abs” and 
“9mag” are the absolute and magnetic heading measured from the compass model and 
finally the Distance t is the distance to the target measured. 
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Figure 80. Qbot heading and distances plots 


C. QBot - Qball COOPERATION WAYPOINT NAVIGATION 

The cooperation of the two vehicles will take place in the form of Leader( Qbot) - 
Follower (QBall). The Qbot navigation will continue taking place as before with the 
motion planner. In this model though, a real time Data Streamer has to be set in order to 
send the actual position to the QBall and then the quadrotor starts his trajectory following 
this positions. 



Display 


Figure 81. “Stream to Qball” subsystem 
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With the above streamer, the waypoint state machine will have as input the 
coordinates of every waypoint instead of having defined by the user. 
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Figure 82. Qball Waypoint State Machine 



The challenge is for the host localization model to recognize both models at the 
same time and send the actual data to the two controllers. That’s why a modification in 
the Qball and Qbot host localization files needed to be made for the cooperation. The 
localization models for each vehicle and the modification for their cooperation are shown 
below: 


|j HOST_qbot_localization * 

File Edit View Simulation Format Tools QUARC Help 


D |"f I External 3 S GH El # 


This model runs on the host ground station and sends 
optltrack data to the Qbot. 

Run this model first before starting the Qbot control model. 

This model can be left running - the Qbot model will reconnect when It Is started. 



Constantl 

Figure 83. Host Qbot Localization 
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This model runs on the host ground station and sends 
joystick and optitrack data to the Qball-X4 model. 

Run this model first before starting the Qball-X4 control model. 

This model can be left running - the Qball-X4 model will reconnect when it is started. 



Display 


Figure 84. Host Qball Localization 


This model runs on the host ground station and sends 
joystick and optitrack data to the Qball-X4 model. 

Run this model first before starting the Qball-X4 control model. 

This model can be left running - the Qball-X4 model will reconnect when it is started. 



Figure 85. Host Qbot - Qball Localization (Modification) 
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The “Optitrack Measurements”subsystem describes how the localization through 
the optitrack motion cameras is done for the two models. The calibration file from the 
cameras is saved in the “Optitrack Point Cloud“ Quarc block. 


(t HO$TJocahzation_Milionii/OptfTracfc Measurements * 



Figure 86. “Optitrack Measurements” Subsystem 


The both vehicles experience the same response as operating alone since the 
cooperation takes place with the same controllers and waypoint state machines/ motion 
planners with the difference that the waypoint positions are sent from the Qbot and not 
form the operator. 
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VI. CONCLUSIONS - FUTURE WORK 


A. CONTRIBUTIONS 

This thesis focused on the design of a framework for achieving cooperation 
between a quadrotor unmanned vehicle, controlled by a LQR controller, and a ground 
robot applying inverse kinematics as a control strategy. Since not much work has been 
done for a controller using inverse dynamics for quadrotors UAVs, a direct method for 
this controller was also introduced. 

The main contributions of this thesis are presented as follows: 

• The successful set up of the laboratory environment is achieved. This 
consisted of the localization system with ten motion capture cameras, the 
ground station, four ground robots and four quadrotors. For this thesis, 
only one quadrotor and one ground robot was used. 

• Applying Newton’s laws, the six degree of freedom mathematical model 
of a quadrotor is derived, including dynamic analysis and design of the 
control inputs. 

• A two dimensional mathematical model for a ground robot is also derived, 
from the basic physics and robot kinematics. 

• A LQR controller is designed for the position and attitude control of the 
quadrotor, taking place in different decoupled channels and under various 
flight tests for improvement of their performance. 

• Applying forward and inverse kinematics, the control of the ground robot 
is achieved. The successful data receiving from robot’s various sensors is 
also tested (Camera, IR detectors, Bumper, Sonar detectors). 

• A waypoint scenario for the navigation of the two vehicles individually or 
simultaneously is implemented. 

• A proposed controller utilizing direct method with inverse dynamics for 
real-time control of the quadrotor UAV is partly designed focusing on 
optimization over quasi optimal solution. 

The ground robot’s camera is placed and tested onboard the quadotor as an 
additional sensor, obtaining successful image. 
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B. CONCLUSIONS 

The following conclusions can be drawn based on the research of this thesis: 

• Quadrotor UAV can be linearized and controlled through linear control 
methods quite successfully if the flight tests are not far from the hover 
conditions; 

• The LQR controller works well enough when the waypoints distances are 
not very close to each other (experiences higher overshoot) and not very 
far away (the trajectory following becomes less precise). 

• The LQR controller can be used as a path following controller together 
with the quasi-optimal trajectory generator for real-time operations 
achieving better performance toward any disturbances or for obstacle 
avoidance. 

• The quadrotor QBall experiences limitations of operation because of the 
battery’s low capacity. Furthermore, the flight performance is degraded as 
the battery capacity and life reduces. 

• The ground robot’s very low speed is a disadvantage for the leader - 
follower scenario of the two vehicles, forcing the operator to reduce the 
quadrotor’s velocity in order to assist for the accomplishment of the 
mission. 

• The robot’s Logitech camera has a slow rate of response, even for the slow 
velocities of the robot. It is not suggested to be used for faster vehicles like 
quadrotors unless it is supposed to operate on a standoff basis 


C. FUTURE WORK 

The recommendations for future work are: 

• Test to the whole length the direct method model and the trajectory 
generator with LQR controller for different scenarios. 

• Achieve the cooperation of two or more quadrotors or two or more ground 
robots involving collision avoidance scenarios. 

• Use multiple quadrotors to observe an object from different angles and all 
of them arrive simultaneously to allow for military reconnaissance 
mission, a search and rescue operation, or an industrial inspection routine. 

• Implement different controllers for path following instead of LQR. 
Integral backstepping, a combination of PID and backstepping was proved 
the best solution for controlling a OS4 quadrotor in EFL’s research so it is 
a possible solution. 
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• Implement a controller for the non-linear dynamic model of the quadrotor, 
to obtain the best performance. 

• Develop a system for the ground vehicle that it will use a terrain map 
provided by the quadrotor. The quadrotor can use a added camera or 
different sensors for image processing in order to construct a map of the 
terrain around the ground robot so as to navigate to a goal position. 

• Finally, since the ground robot consists of a lot of free positions for other 
sensors, test which other sensors widely used in experiments are 
compatible with quanser computer, gumstix. This will allow to use these 
sensors in the quadrotor as well. 
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APPENDIX 


MATLAB / SIMULINK DOCUMENTATION 


setup qball x4.M File 

A MATLAB script that is run whenever the Qball controller model is opened. This script 
runs several other scripts to initialize model and controller parameters. 

% This file runs the calibration and initialization files for the 
Qball. 

% This file is run automatically when the model is loaded or can be run 
% manually. 

% Complimentary filter design for roll/pitch measurements, 
filter_design; 

% LQR gains and other controller parameters for the Qball. 
controller_design; 


filter design.m File 

A script containing the properties of the complementary filter used to estimate the Qball's 
roll and pitch. 

t=10; 

s = tf( 's ' ) ; 

Gg = t A 2*s/(t*s+l) A 2 
Gi = (2*t*s+l)/(t*s+l)"2 


controller design.m File 

A script used to compute the LQR controller gains used in stabilizing the Qball's 
orientation and position. This file is run by the setup_qball_x4.m script. 

LQR Pitch and Roll Controllers M-file 

wnom = 15; 

L = 0.2; 
w = wnom; 

K = 120; 

J = 0.03; 

Jyaw = 0.04; 

CLimit = 0.025; 

M = 1.4; 
g = 9.8; 

Am = [010 

0 0 2*K*L/J 
0 0 -w]; 

Bm = [0 0 w ] ' ; 
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Aobs = Am' ; 

Bobs = eye(3); 

Qobs = diag([.001 10000 .01]); 

Robs = diag([ 1 1 1 ] ) *1; 

Kobs = lqr(Aobs,Bobs,Qobs,Robs) 

Kobs = Kobs'; 

Aobs = Aobs'-Kobs*Bobs'; 
eig(Aobs) 

Bobs = [Bm Kobs] 

Cobs = eye(3) 

Dobs = [0000 
0 0 0 0 
0000]; 

% augment with integrator 
Ai = [Am [0 0 0 ]' 

1000]; 

Bi = [Bm' 0]'; 

Ci = eye (4); 

Di = [0 0 0 0 ] ' ; 

Q = diag( [100 0 22000 10]) ; 

R = 30000; 

ki = lqr(Ai,Bi,Q,R); 
rp_eig = eig(Ai-Bi*ki); 

fprintf( 'ROLL, PITCH DESIGN \n' ) ; 

fprintf( 'P = %5.3f D = %5.3f Actuator = %5.3f I = %5.3f \n\n',ki(l), 
ki (2),ki(3),ki(4)); 
for i = 1:4 

fprintf(' %5.3f + %5.3f i \n ' ,real(rp_eig(i)), imag(rp_eig(i))); 

end; 

LQR Height Controller M-flle 

% Z axis 

vlimith = 0.1; 

Amh = [01 
0 0 ] 

Bmh = [0 4 *K/M] '; 

Cmh = [1 0]; 

Dmh = 0; 

% augment with integrator 
Aih = [Amh [0 0 ] ' 

1 0 0]; 

Bih = [Bmh' 0]'; 

Cih = eye (3); 

Dih = [0 0 0] ' ; 

Q = diag([1 0 50]); 

R = 5000000; 

kh = lqr(Aih,Bih,Q,R); 
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h_eig = eig(Aih-Bih*kh) ; 

fprintf ('************************************************ \n' ); 
fprintf(' Z DESIGN \n'); 

fprintf( 'P = %5.3f D = %5.3f I = %5.3f \n\n',kh(l), kh(2),kh (3)); 
for i = 1:3 

fprintf(' %5.3f + %5.3f i \n ' ,real(h_eig(i)), imag(h_eig(i))); 

end; 

Kph = kh(1); 

Kdh = kh(2); 

Kwh = 0; 

Kih = kh(3); 

LQR Position Controller M-file 

tlimit = 5*pi/180; %max pitch cmd radians 
%tlimit = 15*pi/180; %max pitch cmd radians 
vlimit = 0.3; % max speed cmd in m/sec 
%vlimit =0.5; % max speed cmd in m/sec 

Tau_theta = 1/7; % closed loop time constant for pitch response 
wt =1/Tau_theta; %closed loop theta bandwidth 
kt = 1; 
a = [0100 
0 0 g 0 
0 0 -wt 0 
1000]; 
b = [ 0 0 wt 0 ] ' ; 

q = diag([ 5 2 0 0.1]); 

%q = diag([ 5 2 0 0.1]); 

%r = 50; 
r = 50; 

k = lqr (a, b, q, r) ; 

ac = a-b*k; 

xy_eig = eig(a-b*k); 

Kp = k (1) ; 

Kd = k (2); 

Ki = k (4) ; 

Kw = k (3); 

fprintf(' \n\n X Y Design \n'); 

fprintf ( 'P = %5.3f D = %5.3f Actuator = %5.3f I = %5.3f \n\n',k(l), 
k (2) ,k(3) ,k(4) ) ; 
for i = 1:4 

fprintf(' %5.3f + %5.3f i \n ' ,real(xy_eig(i) ) , imag(xy_eig(i) ) ) ; 

end; 

LQR Yaw Controller M-flle 

Ky = 4; 

Jy = 0.032; 

Amy = [01 
0 0 ] ; 

Bmy = [0 4*Ky/Jy]'; 
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Cmy = eye(2) ; 
Dmy = [0; 0 ] ; 


Qy = diag([1 0.1]); 

Ry = 1000; 

ky = lqr(Amy,Bmy,Qy,Ry); 
h_eigy = eig(Amy-Bmy*ky); 

Kpyaw = ky(1); 

Kdyaw = ky(2); 

Initialize Qball Waypoints.m 

% clear all 
close all 
clc 

height = 301; 

width = 301; 

axs = [-11-11]*0.5; 

x = [axs(1),axs(2)]; 
y = zeros (1, length(x)); 

figure(1), 

plot(x, y),hold on,plot(y, x), 
xlabel( 'X (m) ' ), ylabel ( 'Z (m) ' ) 

% axis([-round(width/2) round(width/2) -round(height/2) 
round(height/2)]) 
axis(axs) 

set(gca, ' Ydir', 'reverse') 
set(gcf, 'Color', [1 1 1]); 

Rx = 0; 

Ry = 0; 

Rt =0; 

n = input ('How many waypoints do you want to define? (max 
if n>10 

n = 10; 

end 

% X and Y waypoint coordinates 
Tx = zeros(11, 1) ; 

Tz = zeros (11, 1) ; 

% Mission state 
% 0 : do nothing, wait 

% 1 : Goto waypoint 

% 2 : Land 

Task = ones(11, 1)*2; % Set all tasks to land initially, 
for i=l: n 

name = strcat (' Enter the coordinate of waypoint no. ' 


10 ): ’); 


num2str(i)) 
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title(name); 

[tx tz] = ginput(l); % in pix 

plot(tx, tz, 's', 'LineWidth', 2, 'MarkerEdgeColor' , 'k' ,... 

'MarkerFaceColor' , 'g' ,... 

'MarkerSize' , 10) ; 

text(tx+5, tz+15, strcat (' Waypoint- 1 , num2str(i))) 

Tx(i) = tx; 

Tz(i) = tz; 

Task (i) = 1; 
if i == n 

Tx(n+1:10) = tx; 

Tz (n+1:10) = tz; 

end 

end 

hold off 

QBall Waypoint State Machine function 

function [h_c, x_c, z_c, s, 1] = fen (x, z, y, Tx, Tz, Task, hgt, t) 

% Constants 

WP_TOL =0.1; % Waypoint tolerance, must be less than this distance (m) 
to arrive at waypoint. 

HGT_TOL = 0.1; 

WP_WAIT_TIME =5; % Wait at waypoint for this many seconds. 

% Missions state 
% 0 : initialize 

% 1 : takeoff 

% 2 : goto waypoint 

% 3 : land 

% 4 : wait at waypoint 

persistent state; 

persistent wp_index; 
persistent to_x; 
persistent to_z; 
persistent t_start; 
persistent t_end; 

if isempty(state) 
state = 0; 

end 

if isempty(wp_index) 
wp_index = 1; 

end 

if isempty(to_x) || isempty(to_z) 
t o_x = 0; 
to_z = 0; 

end 

if isempty(t_start) || isempty(t_end) 

t_start = 0; 
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t_end = 0; 


end 

s = state; 
i = wp_index; 

switch state 
case 0 

h_c = 0; 
x_c = 0; 
z_c = 0; 
if t >= 3 

t o_x = x; 
to_z = z; 
state = 1; 

end 

case 1 

h_c = hgt ; 
x_c = t o_x; 
z_c = to_z; 

if abs(y - hgt) < HGT_TOL 
state = 2; 

end 

case 2 

h_c = hgt ; 

x_c = Tx(wp_index); 

z_c = Tz(wp_index); 

if Task(wp_index) == 1 

if sqrt((x - x_c) A 2 + (z - z_c) A 2) < WP_TOL 
state = 4; 

t_end = t + WP_WAIT_TIME; 

%wp_index = wp_index + 1; 

end 

else 

to_x = Tx(wp_index) ; 
to_z = Tz(wp_index) ; 
state = 3; 
t_start = t; 
t_end = t + 3; 

end 

case 3 


if t >= 

t_end 

h_c 

= 0; 

else 


h_c 

= hgt; 

end 



x_c = t o_x; 
z_c = to_z; 


case 4 
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h_c = hgt ; 
x_c = Tx(wp_index); 
z_c = Tz(wp_index); 
if t >= t_end 
state = 2; 

wp_index = wp_index + 1; 

end 

otherwise 

state = 0; 
h_c = 0; 
x_c = 0; 
z_c = 0; 

end 


Initialize Qbot Waypoints.m 

close all 
clc 

load calibration_file 

height = 301; 

width = 301; 

axs = [-11-11]*0.6; 

x = [axs(1),axs (2)]; 
y = zeros (1, length(x)); 

figure (1), 

plot(x, y),hold on, plot(y, x) , 
xlabel( 'X (m)' ), ylabel('Y (m)' ) 

% axis([-round(width/2) round(width/2) -round(height/2) 
round(height/2)]) 
axis(axs) 

Rx = 0; 

Ry = 0; 

Rt =0; 

n = input ('How many waypoints do you want to define? (max 
if n>10 

n = 10; 

end 

Tx = zeros (10, 1) ; 

Ty = zeros (10, 1) ; 
for i=l:n 

name = strcat (' Enter the coordinate of waypoint no. ', 
title(name); 

[tx ty] = ginput(l); % in pix 

plot(tx, ty, 's', 'LineWidth', 2, 'MarkerEdgeColor' , 'k 

'MarkerFaceColor' , 'g' , . . . 

'Markersize' ,10); 

text(tx+5, ty+15, strcat (' Waypoint-' , num2str(i))) 
Tx(i) = tx; 


10 ): '); 


num2str(i) ) ; 
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Ty(i) = ty; 
if i == n 

Tx(n+1:10) = tx; 

Ty(n+1:10) = ty; 

end 

end 

hold off 

Qbot Mission Planner function 

function [right_vel, left_vel, target_distance, states] = 
motion_planner(target_list, robot_xyt, vlimit, ... 
err_th, delta_t, grid_size, enc_ang, ang_thr, tracking, 

% Initialize output variables 
right_vel = intl6(0); 
left_vel = intl6(0); 
target_distance = -500; 
states = pre_states; 

if ~(abs(robot_xyt(1)) == 5000 || tracking==0) 

% if tracking == 1 

rx = robot_xyt(1); 
ry = robot_xyt(2); 
rtheta = robot_xyt(3); 
target_xy = [rx ry]; 

[n xy] = size(target_list); 
if n==2 && xy==l 

target_xy = [target_list(1,1) target_list(2,1)]; 
n = 1; 

else 

for i=l:n 

if states (1) == i 
for j=l:xy 

target_xy(j) = target_list(i, j); 

end 

end 

end 

end 

tx = target_xy(1); 
ty = target_xy(2); 

target_distance = find_dist(rx, ry, tx, ty); 

if ((states(1) == n) && (target_distance <= err_th)) || 

if states(5) == 1 
rtheta = 0; 

end 

[ang_to_tar, states(3), states(4)]= find_theta(rx, 
rx+100, ry, grid_size, ... 

enc_ang, ang_thr, states (3), states (4)); 
if states (4) == 1 && states (5) == 0 

[right_vel, left_vel] = solve_inv_kin(0, ... 

ang_to_tar, vlimit, delta_t); 

else 
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pre_states) 


states(1) 


ry, rtheta. 



right_vel = intl6(0); 
left_vel = intl6(0); 
states(5) = 1; 

end 

else 

if target_distance <= err_th 

states(1) = pre_states(1) + 1; 

else 

[ang_to_tar, states(3), states(4)]= find_theta(rx, ry, 
rtheta, tx, ty, grid_size, ... 

enc_ang, ang_thr, states (3), states(4)); 

[right_vel, left_vel] = solve_inv_kin(target_distance, 
ang_to_tar, vlimit, delta_t); 

end 

end 

end 

return ; 

"6 

function [y] = check_angle(x) 
y = x; 
if x > pi 

y = x - 2*pi; 
elseif x < -pi 

y = x + 2*pi; 

end 

return ; 

"6 

function dist = find_dist(rx, ry, tx, ty) 
dist = sqrt ( (rx-tx) ^2 + (ry-ty) / '2); 

return ; 

"6 

function [theta, ang_state, rotate_state]= find_theta(rx, ry, rtheta, 

tx, ty, ... 

grid_size, enc_ang, ang_thr, pre_ang_state, pre_rotate_state) 

X = round((tx - rx)/grid_size); 

Y = round((ty - ry)/grid_size); 
theta = atan2(Y, X); 
if pre_rotate_state == 1 

theta = check_angle(theta - pre_ang_state); 

else 

theta = check_angle(theta - check_angle(rtheta)); 

end 

if abs(theta)>=ang_thr 

if pre_rotate_state == 0 
rotate_state = 1; 
ang_state = rtheta; 

else 

rotate_state = pre_rotate_state; 

ang_state = check_angle(pre_ang_state + enc_ang); 

end 

else 

rotate_state = 0; 
ang_state = 0; 

end 

return ; 
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'o 

function [vr, vl] = solve_inv_kin(dist, theta, vlimit, 

d = 252.5; 

vmax = vlimit; %(2); 

wmax = (2*vmax)/d; 

w = theta/delta_t; 

w_sign = sign(w); 

if abs(w) > wmax 

w = w_sign*wmax; 

vr = intl6(round((d*w)/2)); 

vl = intl6(-vr); 

else 

v = dist/delta_t; 
vr_tmp = (2*v + d*w)/2; 

vl_tmp = 2*v - vr_tmp; 

max_of_vrvl = abs(max(vr_tmp, vl_tmp)); 
if max_of_vrvl > vmax 

vr_tmp = (vr_tmp/max_of_vrvl)*vmax; 
vl_tmp = (vl_tmp/max_of_vrvl)*vmax; 

end 

vr = intl6(vr_tmp); 
vl = intl6(vl_tmp); 

end 

return ; 

"6 


delta_t) 
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